From 87254f76557dcbeb529e8a19f1147640d9ec6616 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 1 Aug 2025 13:20:50 +0800 Subject: [PATCH 01/45] Optimize exception message of components analysis --- rmcs_ws/src/rmcs_executor/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index 0de17487..99311f24 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -44,7 +44,7 @@ int main(int argc, char** argv) { std::vector component_descriptions; if (!executor->get_parameter("components", component_descriptions)) - throw std::runtime_error("para"); + throw std::runtime_error("Missing parameter 'components' or config is not found"); std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); for (const auto& component_description : component_descriptions) { From c75f55d4c23ba8acef18ef5a2b60383d9ae4cf6e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 13 Aug 2025 15:50:30 +0800 Subject: [PATCH 02/45] Update docker compose yaml and add .env --- .env | 3 +++ docker-compose.yml | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) create mode 100644 .env diff --git a/.env b/.env new file mode 100644 index 00000000..c28fb80f --- /dev/null +++ b/.env @@ -0,0 +1,3 @@ +CONTAINER_USER=ubuntu +CONTAINER_HOME=/home/${CONTAINER_USER} +HOST_NVIM_DIR=${HOME}/.config/nvim diff --git a/docker-compose.yml b/docker-compose.yml index 384717e7..d59afc5b 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,15 +1,18 @@ services: rmcs-develop: image: qzhhhi/rmcs-develop:latest - container_name: rmcs-develop - user: "developer" - hostname: "developer" + user: "${CONTAINER_USER}" privileged: true + command: > + bash -c " + sudo chown -R ${CONTAINER_USER}:${CONTAINER_USER} ${CONTAINER_HOME}/.config + exec bash + " volumes: - /dev:/dev:bind - /tmp/.X11-unix:/tmp/.X11-unix:bind - /run/user/1000/wayland-0:/run/user/1000/wayland-0 - - ~/.config/nvim/:/home/developer/.config/nvim/:bind + - ${HOST_NVIM_DIR}:${CONTAINER_HOME}/.config/nvim/:bind - .:/workspaces/RMCS:bind environment: - DISPLAY=${DISPLAY} From de060989833180c64e059b765061ff6e9fe387e9 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 12 Sep 2025 18:27:08 +0800 Subject: [PATCH 03/45] Modify submodule url and update scripts --- .gitmodules | 2 +- .script/build-rmcs | 2 +- .script/clean-rmcs | 3 ++- .script/sync-remote | 2 +- .script/template/env_setup.bash | 5 +++-- .script/template/env_setup.zsh | 5 +++-- Dockerfile | 2 +- 7 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.gitmodules b/.gitmodules index 1938ee6e..d4d71be2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,4 +9,4 @@ url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial - url = git@github.com:Alliance-Algorithm/ros2-serial.git + url = https://github.com/Alliance-Algorithm/ros2-serial.git diff --git a/.script/build-rmcs b/.script/build-rmcs index 08d7198e..800a68fc 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -2,6 +2,6 @@ source /opt/ros/jazzy/setup.bash -cd /workspaces/RMCS/rmcs_ws +cd ${RMCS_PATH}/rmcs_ws colcon build --symlink-install --merge-install diff --git a/.script/clean-rmcs b/.script/clean-rmcs index cab27019..3227fd82 100755 --- a/.script/clean-rmcs +++ b/.script/clean-rmcs @@ -1,3 +1,4 @@ #! /bin/bash -rm -rf /workspaces/RMCS/rmcs_ws/build /workspaces/RMCS/rmcs_ws/install /workspaces/RMCS/rmcs_ws/log \ No newline at end of file +rm -rf ${RMCS_PATH}/rmcs_ws/build ${RMCS_PATH}/rmcs_ws/install ${RMCS_PATH}/rmcs_ws/log + diff --git a/.script/sync-remote b/.script/sync-remote index 60a2b816..f9c915fc 100755 --- a/.script/sync-remote +++ b/.script/sync-remote @@ -8,7 +8,7 @@ import subprocess from colorama import Fore, Style -SRC_DIR = "/workspaces/RMCS/rmcs_ws/install" +SRC_DIR = "${RMCS_PATH}/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index aa904464..2906efbf 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,13 +1,14 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.bash if [ -f "/rmcs_install/local_setup.bash" ]; then source /rmcs_install/local_setup.bash -elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.bash" ]; then - source /workspaces/RMCS/rmcs_ws/install/local_setup.bash +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.bash" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.bash fi export RMCS_ROBOT_TYPE="" diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 518afb8f..9de2f1e1 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,13 +1,14 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.zsh if [ -f "/rmcs_install/local_setup.zsh" ]; then source /rmcs_install/local_setup.zsh -elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.zsh" ]; then - source /workspaces/RMCS/rmcs_ws/install/local_setup.zsh +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.zsh" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.zsh fi eval "$(register-python-argcomplete ros2)" diff --git a/Dockerfile b/Dockerfile index 37bf1097..c4a67af1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -123,7 +123,7 @@ 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}:/workspaces/RMCS/.script' >> ~/.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 From 1e700303c885cc0cf30c7472178c00bb11d40a9c Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 20:52:56 +0800 Subject: [PATCH 04/45] Add workflow for updating docker images --- .github/workflows/update-image.yml | 42 ++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 .github/workflows/update-image.yml diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml new file mode 100644 index 00000000..6d4e0dfc --- /dev/null +++ b/.github/workflows/update-image.yml @@ -0,0 +1,42 @@ +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: 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 From a7d319dfef02982f2a37c46a997f2dbb75bd56b8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 21:10:02 +0800 Subject: [PATCH 05/45] Add id_rsa setup for image building --- .github/workflows/update-image.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 6d4e0dfc..a9e77b45 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -25,6 +25,14 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Set up SSH keys + run: | + mkdir -p ~/.ssh + 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: From 26dfdf2e439beb66dfa0cef93278f2d85aa0d342 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 21:22:45 +0800 Subject: [PATCH 06/45] Update workflows: change ssh secrets path --- .github/workflows/update-image.yml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index a9e77b45..71c5538f 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -27,11 +27,10 @@ jobs: - name: Set up SSH keys run: | - mkdir -p ~/.ssh - 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 + 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 From f0393d96ebf12ca8e7333807792188196da035d6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 31 Oct 2025 04:05:47 +0800 Subject: [PATCH 07/45] Update develop scripts and add env param --- .script/build-rmcs | 2 +- .script/template/env_setup.bash | 1 + .script/template/env_setup.zsh | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.script/build-rmcs b/.script/build-rmcs index 800a68fc..ef5d85ff 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -4,4 +4,4 @@ source /opt/ros/jazzy/setup.bash cd ${RMCS_PATH}/rmcs_ws -colcon build --symlink-install --merge-install +colcon build --symlink-install --merge-install $@ diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index 2906efbf..f8ab1dd2 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,6 +1,7 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RCUTILS_COLORIZED_OUTPUT=1 export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.bash diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 9de2f1e1..335250c9 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,5 +1,6 @@ #!/bin/bash +export RCUTILS_COLORIZED_OUTPUT=1 export ROS_LOCALHOST_ONLY=1 export RMCS_PATH="/workspaces/RMCS" From 307530712d3a0a77ba6da035c6a4c4b908683cd6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 13:54:34 +0800 Subject: [PATCH 08/45] Add autoaim visualization script --- .script/play-autoaim | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100755 .script/play-autoaim diff --git a/.script/play-autoaim b/.script/play-autoaim new file mode 100755 index 00000000..e90521dc --- /dev/null +++ b/.script/play-autoaim @@ -0,0 +1,38 @@ +#!/bin/sh + +if [ "$#" -ne 2 ]; then + echo "用法: $0 <用户名> <主机>" + echo "示例: $0 user localhost" + exit 1 +fi + +REMOTE_PLAYER="vlc" + +REMOTE_USER="$1" +REMOTE_HOST="$2" + +REMOTE_PATH="/tmp/auto_aim.sdp" +LOCAL_PATH="/tmp/auto_aim.sdp" + +scp remote:tmp/auto_aim.sdp /tmp/auto_aim.sdp +if [ $? -ne 0 ]; then + echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" + read -r answer + case "$answer" in + [Yy]*) echo "继续执行后续操作…" ;; + *) + echo "已取消。" + exit 1 + ;; + esac +fi + +scp "${REMOTE_PATH}" "${REMOTE_USER}@${REMOTE_HOST}:${LOCAL_PATH}" +if [ $? -ne 0 ]; then + echo "❌ scp 拷贝失败" + exit 1 +fi + +echo "✅ 文件已拷贝到宿主机：${LOCAL_PATH}" + +ssh -f "${REMOTE_USER}@${REMOTE_HOST}" "DISPLAY=:0 ${REMOTE_PLAYER} '${LOCAL_PATH}'" From d0fabc6d9d4c8307cb481074ee1b43162f387a66 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:29:53 +0800 Subject: [PATCH 09/45] Update develop script --- .script/play-autoaim | 8 ++++---- .script/sync-remote | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.script/play-autoaim b/.script/play-autoaim index e90521dc..4a506a2b 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -1,15 +1,15 @@ #!/bin/sh -if [ "$#" -ne 2 ]; then - echo "用法: $0 <用户名> <主机>" - echo "示例: $0 user localhost" +if [ "$#" -ne 1 ]; then + echo "用法: $0 <用户名>" + echo "示例: $0 user" exit 1 fi REMOTE_PLAYER="vlc" REMOTE_USER="$1" -REMOTE_HOST="$2" +REMOTE_HOST="localhost" REMOTE_PATH="/tmp/auto_aim.sdp" LOCAL_PATH="/tmp/auto_aim.sdp" diff --git a/.script/sync-remote b/.script/sync-remote index f9c915fc..7d62782b 100755 --- a/.script/sync-remote +++ b/.script/sync-remote @@ -8,7 +8,7 @@ import subprocess from colorama import Fore, Style -SRC_DIR = "${RMCS_PATH}/rmcs_ws/install" +SRC_DIR = os.getenv("RMCS_PATH") + "/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" From a6ca16a478f49db5019101b30829025b1541b62e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:46:41 +0800 Subject: [PATCH 10/45] Update develop script --- .script/play-autoaim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.script/play-autoaim b/.script/play-autoaim index 4a506a2b..67e86159 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -14,7 +14,7 @@ REMOTE_HOST="localhost" REMOTE_PATH="/tmp/auto_aim.sdp" LOCAL_PATH="/tmp/auto_aim.sdp" -scp remote:tmp/auto_aim.sdp /tmp/auto_aim.sdp +scp remote:${REMOTE_PATH} ${REMOTE_PATH} if [ $? -ne 0 ]; then echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" read -r answer From b4c8ed63e20726db935448c3efc95fd3c0651a0a Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:50:37 +0800 Subject: [PATCH 11/45] Add gstreamer dependency to dockerfile --- Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/Dockerfile b/Dockerfile index c4a67af1..7fe52df3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -18,6 +18,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ vim wget curl unzip \ zsh screen tmux \ usbutils net-tools iputils-ping \ + gstreamer1.0-tools gstreamer1.0-plugins-base gstreamer1.0-plugins-good \ ripgrep htop fzf \ libusb-1.0-0-dev \ libeigen3-dev \ From fa6cec616ad4e49e1051883d9e63ed720209b761 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Tue, 4 Nov 2025 22:34:20 +0800 Subject: [PATCH 12/45] Add device::forcesensor --- .../src/hardware/device/force_sensor.hpp | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp new file mode 100644 index 00000000..dc8a390e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class ForceSensor { +public: + explicit ForceSensor(rmcs_executor::Component& status_component) { + + status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_); + status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_); + } + + void store_status(const uint64_t can_data) { + force_sensor_status_.store(std::bit_cast(can_data), std::memory_order::relaxed); + } + + void update_status() { + auto status = force_sensor_status_.load(std::memory_order::relaxed); + + uint32_t ch1_weight = static_cast(status.ch1_0) << 24 | static_cast(status.ch1_1) << 16 + | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); + uint32_t ch2_weight = static_cast(status.ch2_0) << 24 | static_cast(status.ch2_1) << 16 + | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); + + *weight_ch1_ = std::bit_cast(ch1_weight); + *weight_ch2_ = std::bit_cast(ch2_weight); + } + + static uint64_t generate_command() { return 0x00; } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + struct __attribute__((packed, aligned(8))) ForceSensorStatus { + uint8_t ch1_0; + uint8_t ch1_1; + uint8_t ch1_2; + uint8_t ch1_3; + uint8_t ch2_0; + uint8_t ch2_1; + uint8_t ch2_2; + uint8_t ch2_3; + }; + std::atomic force_sensor_status_{}; + static_assert(decltype(force_sensor_status_)::is_always_lock_free); + + rmcs_executor::Component::OutputInterface weight_ch1_; + rmcs_executor::Component::OutputInterface weight_ch2_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file From a760bd8a66417283d557742ea2cb259f9d511e2e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 4 Nov 2025 23:07:44 +0800 Subject: [PATCH 13/45] Add scripts autocomplete support for zsh --- .script/complete/_play-autoaim | 6 +++ .script/complete/_set-remote | 22 ++++++++++ .script/play-autoaim | 77 ++++++++++++++++++++++++---------- .script/template/env_setup.zsh | 4 ++ 4 files changed, 86 insertions(+), 23 deletions(-) create mode 100644 .script/complete/_play-autoaim create mode 100644 .script/complete/_set-remote diff --git a/.script/complete/_play-autoaim b/.script/complete/_play-autoaim new file mode 100644 index 00000000..7365b535 --- /dev/null +++ b/.script/complete/_play-autoaim @@ -0,0 +1,6 @@ +#compdef play-autoaim + +_arguments \ + '--user[指定远程用户名]:用户名:' \ + '--remote[从设备拉取 SDP 到容器]' \ + '--no-copy[跳过容器到宿主机的拷贝]' 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/play-autoaim b/.script/play-autoaim index 67e86159..8f367049 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -1,38 +1,69 @@ #!/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 -scp remote:${REMOTE_PATH} ${REMOTE_PATH} -if [ $? -ne 0 ]; then - echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" - read -r answer - case "$answer" in - [Yy]*) echo "继续执行后续操作…" ;; +# 参数解析 +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 + ;; *) - echo "已取消。" + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" exit 1 ;; esac -fi + shift +done -scp "${REMOTE_PATH}" "${REMOTE_USER}@${REMOTE_HOST}:${LOCAL_PATH}" -if [ $? -ne 0 ]; then - echo "❌ scp 拷贝失败" +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" exit 1 fi -echo "✅ 文件已拷贝到宿主机：${LOCAL_PATH}" +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 " 继续执行后续操作…" ;; + *) + echo " 已取消。" + exit 1 + ;; + esac + fi +fi + +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 -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.zsh b/.script/template/env_setup.zsh index 335250c9..2f7a0ee6 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_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 From a38c4afd156b30997acd3514b1a3ccefd1aff717 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Tue, 18 Nov 2025 22:27:03 +0800 Subject: [PATCH 14/45] Add dart control framework --- .../src/controller/dart/dart_setttings.cpp | 53 +++++++++ .../src/controller/dart/launch_controller.cpp | 60 ++++++++++ .../rmcs_core/src/hardware/catapult_dart.cpp | 111 ++++++++++++++++++ .../include/rmcs_msgs/dart_launch_status.hpp | 17 +++ 4 files changed, 241 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp new file mode 100644 index 00000000..af5667e8 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp @@ -0,0 +1,53 @@ +#include +#include +#include +#include +#include +#include + +/* +angle controls & launch parameter settings +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +class DartSettings + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartSettings() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) {} + + void update() override {}; + +private: + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + + OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 + OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 + + OutputInterface force_setpoint_; +}; + +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp new file mode 100644 index 00000000..2b291466 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -0,0 +1,60 @@ +#include +#include +#include +#include +#include +#include +#include + +/* +launch controls +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +class DartSettings + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartSettings() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) {} + + void update() override {} + +private: + void reset_all_controls() { + dart_launch_status_ = rmcs_msgs::DartLaunchStatus::DISABLE; + *left_drive_belt_control_torque_ = 0; + *right_drive_belt_control_torque_ = 0; + } + + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + + OutputInterface left_drive_belt_control_torque_; + OutputInterface right_drive_belt_control_torque_; + + rmcs_msgs::DartLaunchStatus dart_launch_status_; +}; + +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp new file mode 100644 index 00000000..4571d601 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -0,0 +1,111 @@ +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/force_sensor.hpp" +#include "librmcs/client/cboard.hpp" +#include +#include +#include +namespace rmcs_core::hardware { +class CatapultDart + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + CatapultDart() + : 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()) + , dart_command_(create_partner_component(get_component_name() + "_command", *this)) + , dr16_(*this) + , pitch_motor_(*this, *dart_command_, "/dart/pitch_motor") + , yaw_motor_(*this, *dart_command_, "/dart/yaw_motor") + , drive_belt_motor_( + {*this, *dart_command_, "/dart/drive_belt/left"}, {*this, *dart_command_, "/dart/drive_belt/right"}) + , force_control_motor_(*this, *dart_command_, "/dart/force_control_motor") + , force_sensor_(*this) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) {} + + ~CatapultDart() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override {} + + void command_update() { + uint16_t can_commands[4]; + + can_commands[0] = 0; + can_commands[1] = 0; + can_commands[2] = 0; + can_commands[3] = 0; + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); + + can_commands[0] = 0; + can_commands[1] = 0; + can_commands[2] = 0; + can_commands[3] = 0; + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + + can_commands[0] = 0; + can_commands[1] = 0; + can_commands[2] = 0; + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); + + can_commands[0] = 0; + can_commands[1] = 0; + can_commands[2] = 0; + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + + transmit_buffer_.trigger_transmission(); + } + +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; + // } + + // 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; + + // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + + // void uart2_receive_callback(const std::byte* data, uint8_t length) override; + + // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + +private: + rclcpp::Logger logger_; + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDart& robot) + : dart(robot) {} + + void update() override { dart.command_update(); } + + CatapultDart& dart; + }; + std::shared_ptr dart_command_; + + device::Dr16 dr16_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor drive_belt_motor_[2]; + device::DjiMotor force_control_motor_; + + device::ForceSensor force_sensor_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; +}; +} // namespace rmcs_core::hardware \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp new file mode 100644 index 00000000..a4f5f9b7 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp @@ -0,0 +1,17 @@ +namespace rmcs_msgs { +enum class DartLaunchStatus { + DISABLE, + INIT, + LOADING, + LAUNCH_READY, +}; +/* +说明： + DISABLE：电机全部失能，遥控器双下 + INIT：初始状态，需要滑台复位 + LOADING：上膛和填装中 + LAUNCH_READY：可发射状态。发射完成后进入INIT +特殊： + LOADING -> INIT：退膛，转动带下去接滑台使其复位，避免空放 +*/ +} // namespace rmcs_msgs \ No newline at end of file From 3fa41f64cb3cb2bafb53adbb789fcb20be0a101d Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Sat, 22 Nov 2025 17:01:48 +0800 Subject: [PATCH 15/45] Add launch_controller --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 19 +++ rmcs_ws/src/rmcs_core/plugins.xml | 12 ++ .../src/controller/dart/launch_controller.cpp | 118 +++++++++++++++++- .../rmcs_core/src/hardware/catapult_dart.cpp | 91 +++++++++----- .../include/rmcs_msgs/dart_launch_status.hpp | 8 +- 5 files changed, 206 insertions(+), 42 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart.yaml diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml new file mode 100644 index 00000000..b1e2c238 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -0,0 +1,19 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDart -> dart_hardware + - rmcs_core::controller::dart::DartLaunchController -> launch_controller + + +dart_hardware: + ros__parameters: + usb_pid: -1 + +launch_controller: + ros__parameters: + belt_velocity: 10.0 + sync_coefficient: 0.5 + b_kp: 1.0 + b_ki: 0.0 + b_kd: 0.0 \ 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..1ef3ddf8 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -98,4 +98,16 @@ Test plugin. + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 2b291466..53facf2f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -1,3 +1,4 @@ +#include "controller/pid/matrix_pid_calculator.hpp" #include #include #include @@ -23,38 +24,145 @@ launch controls */ namespace rmcs_core::controller::dart { -class DartSettings +class DartLaunchController : public rmcs_executor::Component , public rclcpp::Node { public: - DartSettings() + DartLaunchController() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) {} + , logger_(get_logger()) + , drive_belt_pid_calculator_( + get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), get_parameter("b_kd").as_double()) { - void update() override {} + dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + + register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); + register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + } + + void update() override { + using namespace rmcs_msgs; + if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) + && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { + reset_all_controls(); + + } else if (*switch_left_ == Switch::MIDDLE) { + if (dart_launch_status_ == rmcs_msgs::DartLaunchStatus::DISABLE) { + dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; + return; + } + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { + dirve_belt_block_count_ = 0; + drive_belt_control_direction_ = -1; + if (dart_launch_status_ == DartLaunchStatus::INIT) { + dart_launch_status_ = DartLaunchStatus::LOADING; + } + } + + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { + if (dart_launch_status_ == DartLaunchStatus::LAUNCH_READY) { + dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; + trigger_enable_ = true; + } else { + RCLCPP_INFO(logger_, "Dart Hasn't Been Loaded!"); + } + } + + launch_load_control(); + drive_belt_sync_control(); + } + + last_switch_left_ = *switch_left_; + last_switch_right_ = *switch_right_; + + { + if (log_count_++ > 500) { + RCLCPP_INFO(logger_, "dart status: %d", static_cast(dart_launch_status_)); + log_count_ = 0; + } + } // DEBUG + } + double log_count_; private: void reset_all_controls() { dart_launch_status_ = rmcs_msgs::DartLaunchStatus::DISABLE; *left_drive_belt_control_torque_ = 0; *right_drive_belt_control_torque_ = 0; + drive_belt_control_direction_ = 0; } + void launch_load_control() { + using namespace rmcs_msgs; + dirve_belt_block_count_++; + if (dirve_belt_block_count_ >= 500) { + if (drive_belt_control_direction_ == -1) { + drive_belt_control_direction_ = 1; + if (dart_launch_status_ == DartLaunchStatus::LAUNCH_READY) { + trigger_enable_ = true; + dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; + } + } else if (drive_belt_control_direction_ == 1) { + drive_belt_control_direction_ = 0; + if (dart_launch_status_ == DartLaunchStatus::LOADING) { + dart_launch_status_ = DartLaunchStatus::LAUNCH_READY; + } + } + } + } + + void drive_belt_sync_control() { + auto setpoint = drive_belt_control_direction_ * dirve_belt_working_velocity_; + Eigen::Vector2d setpoint_error{setpoint - *left_drive_belt_velocity_, setpoint - *right_drive_belt_velocity_}; + + Eigen::Vector2d relative_velocity{ + *left_drive_belt_velocity_ - *right_drive_belt_velocity_, + *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; + + Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + + *left_drive_belt_control_torque_ = control_torques[0]; + *right_drive_belt_control_torque_ = control_torques[1]; + } + + void trigger_control() {} + + int dirve_belt_block_count_ = 0; + double drive_belt_control_direction_ = 0; + double dirve_belt_working_velocity_; + rclcpp::Logger logger_; InputInterface joystick_right_; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; + rmcs_msgs::Switch last_switch_right_; + rmcs_msgs::Switch last_switch_left_; OutputInterface left_drive_belt_control_torque_; OutputInterface right_drive_belt_control_torque_; + InputInterface left_drive_belt_velocity_; + InputInterface right_drive_belt_velocity_; rmcs_msgs::DartLaunchStatus dart_launch_status_; + bool trigger_enable_ = false; + + pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; + double sync_coefficient_; }; } // namespace rmcs_core::controller::dart #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 4571d601..b6ad21e9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -17,11 +17,20 @@ class CatapultDart , logger_(get_logger()) , dart_command_(create_partner_component(get_component_name() + "_command", *this)) , dr16_(*this) - , pitch_motor_(*this, *dart_command_, "/dart/pitch_motor") - , yaw_motor_(*this, *dart_command_, "/dart/yaw_motor") + , pitch_motor_( + *this, *dart_command_, "/dart/pitch_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) + , yaw_motor_( + *this, *dart_command_, "/dart/yaw_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) + , force_control_motor_( + *this, *dart_command_, "/dart/force_control_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) , drive_belt_motor_( - {*this, *dart_command_, "/dart/drive_belt/left"}, {*this, *dart_command_, "/dart/drive_belt/right"}) - , force_control_motor_(*this, *dart_command_, "/dart/force_control_motor") + {*this, *dart_command_, "/dart/drive_belt/left", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, + {*this, *dart_command_, "/dart/drive_belt/right", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}) , force_sensor_(*this) , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) {} @@ -31,34 +40,29 @@ class CatapultDart event_thread_.join(); } - void update() override {} + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_[0].update_status(); + drive_belt_motor_[1].update_status(); + force_control_motor_.update_status(); + } void command_update() { uint16_t can_commands[4]; - can_commands[0] = 0; - can_commands[1] = 0; - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = 0; - can_commands[2] = 0; + can_commands[0] = pitch_motor_.generate_command(); + can_commands[1] = yaw_motor_.generate_command(); + can_commands[2] = force_control_motor_.generate_command(); can_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - can_commands[0] = 0; - can_commands[1] = 0; + can_commands[0] = drive_belt_motor_[0].generate_command(); + can_commands[1] = drive_belt_motor_[1].generate_command(); can_commands[2] = 0; can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = 0; - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); transmit_buffer_.trigger_transmission(); } @@ -72,15 +76,32 @@ class CatapultDart // return; // } - // 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; + 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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x201) { + pitch_motor_.store_status(can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(can_data); + } else if (can_id == 0x203) { + force_control_motor_.store_status(can_data); + } else if (can_id == 0x205) { + drive_belt_motor_[0].generate_command(); + } else if (can_id == 0x206) { + drive_belt_motor_[1].generate_command(); + } + } // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; // void uart2_receive_callback(const std::byte* data, uint8_t length) override; - // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } private: rclcpp::Logger logger_; @@ -88,11 +109,11 @@ class CatapultDart class DartCommand : public rmcs_executor::Component { public: explicit DartCommand(CatapultDart& robot) - : dart(robot) {} + : dart_(robot) {} - void update() override { dart.command_update(); } + void update() override { dart_.command_update(); } - CatapultDart& dart; + CatapultDart& dart_; }; std::shared_ptr dart_command_; @@ -100,12 +121,16 @@ class CatapultDart device::DjiMotor pitch_motor_; device::DjiMotor yaw_motor_; - device::DjiMotor drive_belt_motor_[2]; device::DjiMotor force_control_motor_; + device::DjiMotor drive_belt_motor_[2]; device::ForceSensor force_sensor_; librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; }; -} // namespace rmcs_core::hardware \ No newline at end of file +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp index a4f5f9b7..183dcb9c 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp @@ -1,9 +1,9 @@ namespace rmcs_msgs { enum class DartLaunchStatus { - DISABLE, - INIT, - LOADING, - LAUNCH_READY, + DISABLE = 0, + INIT = 1, + LOADING = 2, + LAUNCH_READY = 3, }; /* 说明： From f0f4d68109f181313e8ca90b7efd5f4f80134d1f Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Sat, 29 Nov 2025 18:32:55 +0800 Subject: [PATCH 16/45] Add device::TriggerServo --- .../src/controller/dart/servo_test.cpp | 66 +++++++ .../rmcs_core/src/hardware/catapult_dart.cpp | 100 ++++++++++- .../src/hardware/device/trigger_servo.hpp | 166 ++++++++++++++++++ 3 files changed, 328 insertions(+), 4 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp new file mode 100644 index 00000000..9a2f48cc --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::hardware { +class ServoTest + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ServoTest() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + trigger_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); + trigger_free_angle_ = get_parameter("trigger_free_angle").as_int(); + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_output("/dart/trigger_servo/control_angle", trigger_control_angle); + } + + void update() override { + using namespace rmcs_msgs; + if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { + *trigger_control_angle = trigger_lock_angle_; + } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { + *trigger_control_angle = trigger_free_angle_; + } + } + +private: + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + + uint16_t trigger_lock_angle_; + uint16_t trigger_free_angle_; + OutputInterface trigger_control_angle; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ServoTest, rmcs_executor::Component) + +// FF FF 01 03 03 16 E2 +// FF FF 01 04 02 16 02 E0 + +// FF FF 01 03 03 18 E0 +// FF FF 01 04 02 18 02 DE + +// FF FF 01 05 03 1C 00 00 DB +// FF FF 01 05 03 1C 00 01 DA + +// FF FF 01 07 03 41 00 14 00 00 9F +// FF FF 01 07 03 41 00 32 00 00 81 + +// FF FF 01 05 03 41 00 14 A1 +// FF FF 01 04 02 1D 01 DA \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index b6ad21e9..627554fb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,10 +1,15 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" -#include "hardware/device/force_sensor.hpp" +#include "hardware/device/force_sensor_runtime.hpp" +#include "hardware/device/trigger_servo.hpp" #include "librmcs/client/cboard.hpp" +#include #include +#include #include #include +#include + namespace rmcs_core::hardware { class CatapultDart : public rmcs_executor::Component @@ -32,8 +37,15 @@ class CatapultDart {*this, *dart_command_, "/dart/drive_belt/right", device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}) , force_sensor_(*this) + , trigger_servo_(*dart_command_, "/dart/trigger_servo") , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) {} + , event_thread_([this]() { handle_events(); }) { + + trigger_calibrate_subscription_ = create_subscription( + "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(std::move(msg)); + }); + } ~CatapultDart() override { stop_handling_events(); @@ -64,6 +76,16 @@ class CatapultDart can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + if (!trigger_servo_.calibrate_mode()) { + size_t uart_data_length; + std::unique_ptr command_buffer = trigger_servo_.generate_runtime_command(uart_data_length); + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); + + std::string hex_string = bytes_to_hex_string(command_buffer.get(), uart_data_length); + RCLCPP_INFO(this->get_logger(), "UART2(length: %zu): [ %s ]", uart_data_length, hex_string.c_str()); + } + transmit_buffer_.trigger_transmission(); } @@ -97,13 +119,80 @@ class CatapultDart // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; - // void uart2_receive_callback(const std::byte* data, uint8_t length) override; + void uart2_receive_callback(const std::byte* data, uint8_t length) override { + bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); + if (!success) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + + std::string hex_string = bytes_to_hex_string(data, length); + RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, hex_string.c_str()); + } void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { dr16_.store_status(uart_data, uart_data_length); } private: + void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { + /* + 标定命令格式： + ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" + 替换data值就行 + */ + trigger_servo_.set_calibrate_mode(msg->data); + + std::unique_ptr command_buffer; + size_t command_length = 0; + if (msg->data == 0) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); + } else if (msg->data == 1) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); + } else if (msg->data == 2) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); + } else if (msg->data == 3) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); + } else if (msg->data == 4) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); + } else if (msg->data == 5) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); + } else if (msg->data == 6) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); + } + + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); + + std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); + RCLCPP_INFO(this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, hex_string.c_str()); + } + + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) { + return "[]"; + } + + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + + for (size_t i = 0; i < size; ++i) { + ss << std::setw(2) << static_cast(data[i]) << " "; + } + + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') { + result.pop_back(); + } + return result; + } + rclcpp::Logger logger_; class DartCommand : public rmcs_executor::Component { @@ -124,7 +213,10 @@ class CatapultDart device::DjiMotor force_control_motor_; device::DjiMotor drive_belt_motor_[2]; - device::ForceSensor force_sensor_; + device::ForceSensorRuntime force_sensor_; + device::TriggerServo trigger_servo_; + + rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp new file mode 100644 index 00000000..bf695f24 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, +}; + +class TriggerServo { +public: + TriggerServo(rmcs_executor::Component& command_component, const std::string& name) { + command_component.register_input(name + "/control_angle", control_angle_); + } + bool calibrate_mode() const { return is_calibrate_mode_; } + + void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + + std::unique_ptr generate_runtime_command(size_t& output_length) { + size_t size = sizeof(WrightControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0x01; + command.data_length = 0x07; + command.command_type = 0x03; + command.command_address = 0x2A; + command.control_angle[0] = static_cast(*control_angle_ >> 8); + command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); + command.control_runtime[0] = 0x00; + command.control_runtime[1] = 0x00; + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + + // RCLCPP_INFO( + // logger, "%x, %x, %x, %x", *control_angle_, *control_angle_ >> 8, static_cast(*control_angle_ >> + // 8), static_cast(*control_angle_ & 0x00FF)); + return buffer; + } + + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + uint8_t* command = nullptr; + size_t command_size = 0; + + if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { + command_size = sizeof(switch_servo_mode_); + command = switch_servo_mode_; + } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { + command_size = sizeof(switch_motor_mode_); + command = switch_motor_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { + command_size = sizeof(switch_motor_forward_mode_); + command = switch_motor_forward_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { + command_size = sizeof(switch_motor_reverse_mode_); + command = switch_motor_reverse_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { + command_size = sizeof(motor_mode_runtime_control_); + command = motor_mode_runtime_control_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { + command_size = sizeof(motor_mode_disable_control_); + command = motor_mode_disable_control_; + } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { + command_size = sizeof(read_angle_command_); + command = read_angle_command_; + } + + std::unique_ptr buffer = std::make_unique(command_size); + std::memcpy(buffer.get(), command, command_size); + output_length = command_size; + + return buffer; + } + + static bool + calibrate_current_angle(const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != sizeof(ReadAngleReceiveData)) { + RCLCPP_INFO(logger, "UART data length not match"); + return false; + } + + ReadAngleReceiveData package; + uint8_t checksum; + + std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); + std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + + if (package.header[0] != 0xFF || package.header[1] != 0xF5) { + RCLCPP_INFO(logger, "UART data header not match"); + return false; + } + + if (checksum != package.calculate_checksum()) { + RCLCPP_INFO(logger, "Checksum error: receive:%x,calc:%x", checksum, package.calculate_checksum()); + return false; + } + + uint16_t current_angle = + static_cast(package.current_angle[0]) << 8 | static_cast(package.current_angle[1]); + RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); + return true; + } + +private: + struct __attribute__((packed)) WrightControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t control_angle[2]; + uint8_t control_runtime[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + control_angle[1] + + control_runtime[0] + control_runtime[1]; + return ~check; + } + }; + + struct __attribute__((packed)) ReadAngleReceiveData { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t servo_status; + uint8_t current_angle[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; + return ~check; + } + }; + uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02, 0xBE}; + + uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x01, 0xDA}; + uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x00, 0xDB}; + + uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x00, 0xDA}; + uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x01, 0xD9}; + + uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA1}; + uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB5}; + + rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + + std::atomic is_calibrate_mode_ = false; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file From 201574fbadf98f1a4c2a4ef9c4915c249d292509 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Sat, 29 Nov 2025 23:04:13 +0800 Subject: [PATCH 17/45] Supplementary trigger_servo example and test program --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 8 +++++++- rmcs_ws/src/rmcs_core/plugins.xml | 3 +++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index b1e2c238..0cfdd1df 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -4,6 +4,7 @@ rmcs_executor: components: - rmcs_core::hardware::CatapultDart -> dart_hardware - rmcs_core::controller::dart::DartLaunchController -> launch_controller + - rmcs_core::hardware::ServoTest -> servo_test dart_hardware: @@ -16,4 +17,9 @@ launch_controller: sync_coefficient: 0.5 b_kp: 1.0 b_ki: 0.0 - b_kd: 0.0 \ No newline at end of file + b_kd: 0.0 + +servo_test: + ros__parameters: + trigger_lock_angle: 0x0CC5 + trigger_free_angle: 0x0389 \ 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 1ef3ddf8..19746188 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -110,4 +110,7 @@ Test plugin. + + Test plugin. + \ No newline at end of file From 576d3e6c6255a804f6b38661539426b90b4fe779 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Sat, 29 Nov 2025 23:04:59 +0800 Subject: [PATCH 18/45] Optimized launch control --- .../src/controller/dart/launch_controller.cpp | 131 +++++++++++------- .../include/rmcs_msgs/dart_launch_stage.hpp | 20 +++ .../include/rmcs_msgs/dart_launch_status.hpp | 17 --- 3 files changed, 99 insertions(+), 69 deletions(-) create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp delete mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 53facf2f..1e25d2da 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -1,11 +1,14 @@ #include "controller/pid/matrix_pid_calculator.hpp" +#include +#include #include #include #include #include #include -#include +#include #include +#include /* launch controls @@ -36,6 +39,10 @@ class DartLaunchController dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + + launch_trigger_angle_ = get_parameter("launch_trigger_angle").as_int(); + launch_lock_angle_ = get_parameter("launch_lock_angle").as_int(); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); @@ -47,82 +54,89 @@ class DartLaunchController register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + + register_output("/dart/trigger_servo/control_angle", trigger_control_angle); } void update() override { using namespace rmcs_msgs; + if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { + *launch_stage_ = DartLaunchStages::DISABLE; reset_all_controls(); } else if (*switch_left_ == Switch::MIDDLE) { - if (dart_launch_status_ == rmcs_msgs::DartLaunchStatus::DISABLE) { - dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; - return; + double control_velocity = 0; + + if (last_launch_stage_ == DartLaunchStages::DISABLE) { + *launch_stage_ = DartLaunchStages::RESETTING; } + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - dirve_belt_block_count_ = 0; - drive_belt_control_direction_ = -1; - if (dart_launch_status_ == DartLaunchStatus::INIT) { - dart_launch_status_ = DartLaunchStatus::LOADING; + if (last_launch_stage_ == DartLaunchStages::INIT) { + *launch_stage_ = DartLaunchStages::LOADING; + control_velocity = dirve_belt_working_velocity_; + } else if (last_launch_stage_ == DartLaunchStages::READY) { + *launch_stage_ = DartLaunchStages::CANCEL; + control_velocity = dirve_belt_working_velocity_; } - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - if (dart_launch_status_ == DartLaunchStatus::LAUNCH_READY) { - dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; - trigger_enable_ = true; + } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { + if (last_launch_stage_ == DartLaunchStages::READY) { + *launch_stage_ = DartLaunchStages::INIT; + trigger_lock_flag_ = false; } else { - RCLCPP_INFO(logger_, "Dart Hasn't Been Loaded!"); + RCLCPP_INFO(logger_, "Dart has't been loaded !"); } } - launch_load_control(); - drive_belt_sync_control(); + if (blocking_detection()) { + if (last_launch_stage_ == DartLaunchStages::LOADING) { + *launch_stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = true; + dirve_belt_block_count_ = 0; + control_velocity = -dirve_belt_working_velocity_; + + } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { + *launch_stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = false; + dirve_belt_block_count_ = 0; + control_velocity = -dirve_belt_working_velocity_; + + } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { + *launch_stage_ = trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; + dirve_belt_block_count_ = 0; + control_velocity = 0; + } + } + drive_belt_sync_control(control_velocity); } + *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; + last_switch_left_ = *switch_left_; last_switch_right_ = *switch_right_; - - { - if (log_count_++ > 500) { - RCLCPP_INFO(logger_, "dart status: %d", static_cast(dart_launch_status_)); - log_count_ = 0; - } - } // DEBUG + last_launch_stage_ = *launch_stage_; } + double log_count_; private: void reset_all_controls() { - dart_launch_status_ = rmcs_msgs::DartLaunchStatus::DISABLE; + *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; *left_drive_belt_control_torque_ = 0; *right_drive_belt_control_torque_ = 0; - drive_belt_control_direction_ = 0; } - void launch_load_control() { - using namespace rmcs_msgs; - dirve_belt_block_count_++; - if (dirve_belt_block_count_ >= 500) { - if (drive_belt_control_direction_ == -1) { - drive_belt_control_direction_ = 1; - if (dart_launch_status_ == DartLaunchStatus::LAUNCH_READY) { - trigger_enable_ = true; - dart_launch_status_ = rmcs_msgs::DartLaunchStatus::INIT; - } - } else if (drive_belt_control_direction_ == 1) { - drive_belt_control_direction_ = 0; - if (dart_launch_status_ == DartLaunchStatus::LOADING) { - dart_launch_status_ = DartLaunchStatus::LAUNCH_READY; - } - } + void drive_belt_sync_control(double set_velocity) { + if (set_velocity == 0) { + *left_drive_belt_control_torque_ = 0; + *right_drive_belt_control_torque_ = 0; + return; } - } - void drive_belt_sync_control() { - auto setpoint = drive_belt_control_direction_ * dirve_belt_working_velocity_; - Eigen::Vector2d setpoint_error{setpoint - *left_drive_belt_velocity_, setpoint - *right_drive_belt_velocity_}; + Eigen::Vector2d setpoint_error{ + set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; Eigen::Vector2d relative_velocity{ *left_drive_belt_velocity_ - *right_drive_belt_velocity_, @@ -130,14 +144,20 @@ class DartLaunchController Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - *left_drive_belt_control_torque_ = control_torques[0]; - *right_drive_belt_control_torque_ = control_torques[1]; + *left_drive_belt_control_torque_ = std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_drive_belt_control_torque_ = std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); } - void trigger_control() {} + bool blocking_detection() { + if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) + || (abs(*right_drive_belt_velocity_) < 0.5 && abs(*right_drive_belt_control_torque_) > 0.5)) { + dirve_belt_block_count_++; + } + + return dirve_belt_block_count_ > 1000 ? true : false; + } int dirve_belt_block_count_ = 0; - double drive_belt_control_direction_ = 0; double dirve_belt_working_velocity_; rclcpp::Logger logger_; @@ -154,8 +174,15 @@ class DartLaunchController InputInterface left_drive_belt_velocity_; InputInterface right_drive_belt_velocity_; - rmcs_msgs::DartLaunchStatus dart_launch_status_; - bool trigger_enable_ = false; + double max_control_torque_; + + OutputInterface launch_stage_; + rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + + uint16_t launch_lock_angle_; + uint16_t launch_trigger_angle_; + bool trigger_lock_flag_ = false; + OutputInterface trigger_control_angle; pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; double sync_coefficient_; diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp new file mode 100644 index 00000000..9ce532b9 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp @@ -0,0 +1,20 @@ +namespace rmcs_msgs { +enum class DartLaunchStages { + DISABLE, + INIT, + LOADING, + RESETTING, + READY, + CANCEL, +}; +/* +说明： + DISABLE：电机全部失能，遥控器双下 + INIT：初始状态，需要滑台复位 + LOADING：上膛和填装中 + RESETTING: 同步带复位中 + READY：可发射状态。发射完成后进入INIT +特殊： + READY -> INIT：退膛，转动带下去接滑台使其复位，避免空放 +*/ +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp deleted file mode 100644 index 183dcb9c..00000000 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_status.hpp +++ /dev/null @@ -1,17 +0,0 @@ -namespace rmcs_msgs { -enum class DartLaunchStatus { - DISABLE = 0, - INIT = 1, - LOADING = 2, - LAUNCH_READY = 3, -}; -/* -说明： - DISABLE：电机全部失能，遥控器双下 - INIT：初始状态，需要滑台复位 - LOADING：上膛和填装中 - LAUNCH_READY：可发射状态。发射完成后进入INIT -特殊： - LOADING -> INIT：退膛，转动带下去接滑台使其复位，避免空放 -*/ -} // namespace rmcs_msgs \ No newline at end of file From 26400cdec0e44e710b4485ea8c946961611bbe2e Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Tue, 2 Dec 2025 23:01:56 +0800 Subject: [PATCH 19/45] Modified force_sensor, testing zero drift --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 11 +- .../src/rmcs_bringup/config/force-sensor.yaml | 12 ++ rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../src/controller/dart/dart_setttings.cpp | 52 ++++++- .../rmcs_core/src/hardware/catapult_dart.cpp | 19 ++- .../src/hardware/device/force_sensor.hpp | 59 -------- .../hardware/device/force_sensor_runtime.hpp | 61 ++++++++ .../src/hardware/force_sensor_receive.cpp | 37 +++++ .../src/hardware/force_sensor_setting.cpp | 99 +++++++++++++ .../src/hardware/force_sensor_test.cpp | 134 ++++++++++++++++++ .../include/rmcs_msgs/force_sensor_mode.hpp | 17 +++ 11 files changed, 431 insertions(+), 73 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/force-sensor.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/force_sensor_mode.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index 0cfdd1df..742446e4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -4,8 +4,8 @@ rmcs_executor: components: - rmcs_core::hardware::CatapultDart -> dart_hardware - rmcs_core::controller::dart::DartLaunchController -> launch_controller - - rmcs_core::hardware::ServoTest -> servo_test - + # - rmcs_core::hardware::ServoTest -> servo_test + - rmcs_core::controller::dart::DartSettings -> dart_settings dart_hardware: ros__parameters: @@ -22,4 +22,9 @@ launch_controller: servo_test: ros__parameters: trigger_lock_angle: 0x0CC5 - trigger_free_angle: 0x0389 \ No newline at end of file + trigger_free_angle: 0x0389 + +dart_settings: + ros__parameters: + log_enable: true + screw_max_velocity: 10.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/force-sensor.yaml b/rmcs_ws/src/rmcs_bringup/config/force-sensor.yaml new file mode 100644 index 00000000..7faee24c --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/force-sensor.yaml @@ -0,0 +1,12 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::ForceSensorTest -> force_sensor_test + - rmcs_core::hardware::ForceSensorReceive -> force_sensor_receive + + +force_sensor_test: + ros__parameters: + usb_pid: -1 + diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 19746188..f0cca872 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -110,6 +110,9 @@ Test plugin. + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp index af5667e8..5a072816 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp @@ -28,11 +28,48 @@ class DartSettings public: DartSettings() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) {} + , logger_(get_logger()) { + log_enable_ = get_parameter("log_enable").as_bool(); + force_screw_max_velocity_ = get_parameter("screw_max_velocity").as_double(); - void update() override {}; + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_weight_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_weight_); + + register_output("/dart/force_control_motor/control_velocity", force_control_velocity_); + } + + void update() override { + if (*switch_left_ == rmcs_msgs::Switch::UP) { + if (*switch_right_ == rmcs_msgs::Switch::DOWN) { + *force_control_velocity_ = joystick_right_->x() * force_screw_max_velocity_; + } + if (*switch_right_ == rmcs_msgs::Switch::MIDDLE) { + // angle control + } + } else { + reset(); + } + + if (log_enable_) { + measure_log(); + } + } private: + void reset() { *force_control_velocity_ = 0; } + + void measure_log() { + if (log_count_++ > 100) { + log_count_ = 0; + RCLCPP_INFO(logger_, "ch1: %d | ch2: %d", *force_sensor_ch1_weight_, *force_sensor_ch2_weight_); + } + } + rclcpp::Logger logger_; InputInterface joystick_right_; @@ -40,10 +77,15 @@ class DartSettings InputInterface switch_right_; InputInterface switch_left_; - OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 - OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 + // OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 + // OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 + + InputInterface force_sensor_ch1_weight_, force_sensor_ch2_weight_; + OutputInterface force_control_velocity_; + double force_screw_max_velocity_; - OutputInterface force_setpoint_; + bool log_enable_ = false; + int log_count_ = 0; }; } // namespace rmcs_core::controller::dart diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 627554fb..981c6978 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -59,11 +59,14 @@ class CatapultDart drive_belt_motor_[0].update_status(); drive_belt_motor_[1].update_status(); force_control_motor_.update_status(); + force_sensor_.update_status(); } void command_update() { uint16_t can_commands[4]; + transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); + can_commands[0] = pitch_motor_.generate_command(); can_commands[1] = yaw_motor_.generate_command(); can_commands[2] = force_control_motor_.generate_command(); @@ -90,13 +93,17 @@ class CatapultDart } 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 { + 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 (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - // return; - // } + if (can_id == 0x302) { + force_sensor_.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, diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp deleted file mode 100644 index dc8a390e..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class ForceSensor { -public: - explicit ForceSensor(rmcs_executor::Component& status_component) { - - status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_); - status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_); - } - - void store_status(const uint64_t can_data) { - force_sensor_status_.store(std::bit_cast(can_data), std::memory_order::relaxed); - } - - void update_status() { - auto status = force_sensor_status_.load(std::memory_order::relaxed); - - uint32_t ch1_weight = static_cast(status.ch1_0) << 24 | static_cast(status.ch1_1) << 16 - | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); - uint32_t ch2_weight = static_cast(status.ch2_0) << 24 | static_cast(status.ch2_1) << 16 - | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); - - *weight_ch1_ = std::bit_cast(ch1_weight); - *weight_ch2_ = std::bit_cast(ch2_weight); - } - - static uint64_t generate_command() { return 0x00; } - -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - struct __attribute__((packed, aligned(8))) ForceSensorStatus { - uint8_t ch1_0; - uint8_t ch1_1; - uint8_t ch1_2; - uint8_t ch1_3; - uint8_t ch2_0; - uint8_t ch2_1; - uint8_t ch2_2; - uint8_t ch2_3; - }; - std::atomic force_sensor_status_{}; - static_assert(decltype(force_sensor_status_)::is_always_lock_free); - - rmcs_executor::Component::OutputInterface weight_ch1_; - rmcs_executor::Component::OutputInterface weight_ch2_; -}; - -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp new file mode 100644 index 00000000..e816a684 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor_runtime.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class ForceSensorRuntime { +public: + explicit ForceSensorRuntime(rmcs_executor::Component& status_component) { + status_component.register_output("/force_sensor/channel_1/weight", channel_1_weight_, nan_); + status_component.register_output("/force_sensor/channel_2/weight", channel_2_weight_, nan_); + } + + void store_status(const uint64_t can_data) { + force_sensor_status_.store(std::bit_cast(can_data), std::memory_order::relaxed); + } + + void update_status() { + auto status = force_sensor_status_.load(std::memory_order::relaxed); + + uint32_t ch1_weight = + static_cast(status.ch1_weight_0) << 24 | static_cast(status.ch1_weight_1) << 16 + | static_cast(status.ch1_weight_2) << 8 | static_cast(status.ch1_weight_3); + uint32_t ch2_weight = + static_cast(status.ch2_weight_0) << 24 | static_cast(status.ch2_weight_1) << 16 + | static_cast(status.ch2_weight_2) << 8 | static_cast(status.ch2_weight_3); + + *channel_1_weight_ = std::bit_cast(ch1_weight); + *channel_2_weight_ = std::bit_cast(ch2_weight); + } + + static uint64_t generate_command() { return 0x00; } + static uint64_t generate_zero_calibration_command() { return 0x0F; } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + struct __attribute__((packed, aligned(8))) ForceSensorStatus { + uint8_t ch1_weight_0; + uint8_t ch1_weight_1; + uint8_t ch1_weight_2; + uint8_t ch1_weight_3; + uint8_t ch2_weight_0; + uint8_t ch2_weight_1; + uint8_t ch2_weight_2; + uint8_t ch2_weight_3; + }; + std::atomic force_sensor_status_{}; + static_assert(decltype(force_sensor_status_)::is_always_lock_free); + + rmcs_executor::Component::OutputInterface channel_1_weight_; + rmcs_executor::Component::OutputInterface channel_2_weight_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp new file mode 100644 index 00000000..8b53eaf2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp @@ -0,0 +1,37 @@ +#include +#include +#include +#include +namespace rmcs_core::hardware { +class ForceSensorReceive + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ForceSensorReceive() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); + } + + void update() override { + log_count_++; + if (log_count_ == 100) { + log_count_ = 0; + RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); + } + }; + +private: + rclcpp::Logger logger_; + + int log_count_ = 0; + + InputInterface force_sensor_ch1_data_; + InputInterface force_sensor_ch2_data_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorReceive, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp new file mode 100644 index 00000000..b8a50d2d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp @@ -0,0 +1,99 @@ +#include "hardware/device/force_sensor_runtime.hpp" +#include "librmcs/client/cboard.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class ForceSensorSetting + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { + +public: + ForceSensorSetting() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} + , robot_command_(create_partner_component(get_component_name() + "_command", *this)) + , force_sensor_(*this) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) {} + + ~ForceSensorSetting() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { force_sensor_.update_status(); } + + void command_update() { + + count_++; + if (count_ == 100) { + count_ = 0; + transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); + } + + transmit_buffer_.trigger_transmission(); + } + + int count_ = 0; + +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 == 0x302) { + force_sensor_.store_status(can_data); + sensor_.store(std::bit_cast(can_data), std::memory_order_relaxed); + auto data = sensor_.load(std::memory_order_relaxed); + + RCLCPP_INFO(get_logger(), "ch1: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch1_0, data.ch1_1, data.ch1_2, data.ch1_3); + RCLCPP_INFO(get_logger(), "ch2: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch2_0, data.ch2_1, data.ch2_2, data.ch2_3); + } + } + + struct sensor_data { + uint8_t ch1_0; + uint8_t ch1_1; + uint8_t ch1_2; + uint8_t ch1_3; + uint8_t ch2_0; + uint8_t ch2_1; + uint8_t ch2_2; + uint8_t ch2_3; + }; + + std::atomic sensor_{}; + +private: + class RoboCommand : public rmcs_executor::Component { + public: + explicit RoboCommand(ForceSensorSetting& robot) + : robot_(robot) {} + + void update() override { robot_.command_update(); } + + ForceSensorSetting& robot_; + }; + std::shared_ptr robot_command_; + + device::ForceSensorRuntime force_sensor_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorSetting, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp new file mode 100644 index 00000000..0527411d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp @@ -0,0 +1,134 @@ +#include "hardware/device/force_sensor_runtime.hpp" +#include "librmcs/client/cboard.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class ForceSensorTest + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { + +public: + ForceSensorTest() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} + , robot_command_(create_partner_component(get_component_name() + "_command", *this)) + , force_sensor_(*this) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) {} + + ~ForceSensorTest() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { force_sensor_.update_status(); } + + void command_update() { + // uint16_t can_commands[4]; + + // can_commands[0] = 0; + // can_commands[1] = 0; + // can_commands[2] = 0; + // can_commands[3] = 0; + // transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); + + // can_commands[0] = 0; + // can_commands[1] = 0; + // can_commands[2] = 0; + // can_commands[3] = 0; + // transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + + // can_commands[0] = 0; + // can_commands[1] = 0; + // can_commands[2] = 0; + // can_commands[3] = 0; + // transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); + + // can_commands[0] = 0; + // can_commands[1] = 0; + // can_commands[2] = 0; + // can_commands[3] = 0; + // transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + + count_++; + if (count_ == 100) { + count_ = 0; + transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); + } + + transmit_buffer_.trigger_transmission(); + } + + int count_ = 0; + +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 == 0x302) { + force_sensor_.store_status(can_data); + sensor_.store(std::bit_cast(can_data), std::memory_order_relaxed); + auto data = sensor_.load(std::memory_order_relaxed); + + RCLCPP_INFO(get_logger(), "ch1: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch1_0, data.ch1_1, data.ch1_2, data.ch1_3); + RCLCPP_INFO(get_logger(), "ch2: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch2_0, data.ch2_1, data.ch2_2, data.ch2_3); + } + } + + struct sensor_data { + uint8_t ch1_0; + uint8_t ch1_1; + uint8_t ch1_2; + uint8_t ch1_3; + uint8_t ch2_0; + uint8_t ch2_1; + uint8_t ch2_2; + uint8_t ch2_3; + }; + + std::atomic sensor_{}; + + // 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; + + // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + + // void uart2_receive_callback(const std::byte* data, uint8_t length) override; + + // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + +private: + class RoboCommand : public rmcs_executor::Component { + public: + explicit RoboCommand(ForceSensorTest& robot) + : robot_(robot) {} + + void update() override { robot_.command_update(); } + + ForceSensorTest& robot_; + }; + std::shared_ptr robot_command_; + + device::ForceSensorRuntime force_sensor_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/force_sensor_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/force_sensor_mode.hpp new file mode 100644 index 00000000..cb17542d --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/force_sensor_mode.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ForceSensorCommand : uint8_t { + UNKNOWN = 0, + PEEL = 1, + ZERO_CALIBRATION = 2, + WEIGHT_READ = 3, + COMMUNICATION_SETTING = 4, + MEASUREMENT_SETTING = 5, + SENSOR_CALIBRATION = 6 +}; + +} // namespace rmcs_msgs \ No newline at end of file From b3b6fa5723ff46ff682219460cabef62d2fd622e Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 3 Dec 2025 08:56:22 +0800 Subject: [PATCH 20/45] LaunchController test AC --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 20 ++++++-- .../src/controller/dart/launch_controller.cpp | 51 ++++++++++++------- .../rmcs_core/src/hardware/catapult_dart.cpp | 40 ++++++++++----- 3 files changed, 79 insertions(+), 32 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index 742446e4..cb9df80c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -6,6 +6,17 @@ rmcs_executor: - rmcs_core::controller::dart::DartLaunchController -> launch_controller # - rmcs_core::hardware::ServoTest -> servo_test - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + +broadcaster: + ros__parameters: + forward_list: + - /dart/force_control_motor/control_velocity + - /dart/drive_belt/left/control_torque + - /dart/drive_belt/left/velocity + - /dart/drive_belt/right/control_torque + - /dart/drive_belt/right/velocity + - /dart/force_control_motor/velocity dart_hardware: ros__parameters: @@ -18,13 +29,16 @@ launch_controller: b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 + max_control_torque: 1.0 + trigger_free_angle: 0x0E2A + trigger_lock_angle: 0x0FC1 servo_test: ros__parameters: - trigger_lock_angle: 0x0CC5 - trigger_free_angle: 0x0389 + trigger_free_angle: 0x0E2A + trigger_lock_angle: 0x0FC1 dart_settings: ros__parameters: - log_enable: true + log_enable: false screw_max_velocity: 10.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 1e25d2da..5aa8b28f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -32,17 +32,20 @@ class DartLaunchController , public rclcpp::Node { public: DartLaunchController() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) , drive_belt_pid_calculator_( - get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), get_parameter("b_kd").as_double()) { + get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), + get_parameter("b_kd").as_double()) { dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); sync_coefficient_ = get_parameter("sync_coefficient").as_double(); max_control_torque_ = get_parameter("max_control_torque").as_double(); - launch_trigger_angle_ = get_parameter("launch_trigger_angle").as_int(); - launch_lock_angle_ = get_parameter("launch_lock_angle").as_int(); + launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); + launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); @@ -53,7 +56,8 @@ class DartLaunchController register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); - register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + register_output( + "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); register_output("/dart/trigger_servo/control_angle", trigger_control_angle); } @@ -67,7 +71,6 @@ class DartLaunchController reset_all_controls(); } else if (*switch_left_ == Switch::MIDDLE) { - double control_velocity = 0; if (last_launch_stage_ == DartLaunchStages::DISABLE) { *launch_stage_ = DartLaunchStages::RESETTING; @@ -76,10 +79,8 @@ class DartLaunchController if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { if (last_launch_stage_ == DartLaunchStages::INIT) { *launch_stage_ = DartLaunchStages::LOADING; - control_velocity = dirve_belt_working_velocity_; } else if (last_launch_stage_ == DartLaunchStages::READY) { *launch_stage_ = DartLaunchStages::CANCEL; - control_velocity = dirve_belt_working_velocity_; } } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { if (last_launch_stage_ == DartLaunchStages::READY) { @@ -95,21 +96,31 @@ class DartLaunchController *launch_stage_ = DartLaunchStages::RESETTING; trigger_lock_flag_ = true; dirve_belt_block_count_ = 0; - control_velocity = -dirve_belt_working_velocity_; } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { *launch_stage_ = DartLaunchStages::RESETTING; trigger_lock_flag_ = false; dirve_belt_block_count_ = 0; - control_velocity = -dirve_belt_working_velocity_; } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { - *launch_stage_ = trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; + *launch_stage_ = + trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; dirve_belt_block_count_ = 0; - control_velocity = 0; } } + double control_velocity = 0; + + if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { + control_velocity = -dirve_belt_working_velocity_; + } else if ( + *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING + || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { + control_velocity = dirve_belt_working_velocity_; + } else { + control_velocity = 0; + } drive_belt_sync_control(control_velocity); + RCLCPP_INFO(logger_, "%lf", control_velocity); } *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; @@ -117,10 +128,13 @@ class DartLaunchController last_switch_left_ = *switch_left_; last_switch_right_ = *switch_right_; last_launch_stage_ = *launch_stage_; + // RCLCPP_INFO(logger_, "%lf | %lf", *right_drive_belt_velocity_, + // *left_drive_belt_velocity_); + // RCLCPP_INFO( + // logger_, "%d | %lf | %lf", static_cast(*launch_stage_), + // *left_drive_belt_control_torque_, *right_drive_belt_control_torque_); } - double log_count_; - private: void reset_all_controls() { *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; @@ -144,13 +158,16 @@ class DartLaunchController Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - *left_drive_belt_control_torque_ = std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_drive_belt_control_torque_ = std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + *left_drive_belt_control_torque_ = + std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_drive_belt_control_torque_ = + std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); } bool blocking_detection() { if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) - || (abs(*right_drive_belt_velocity_) < 0.5 && abs(*right_drive_belt_control_torque_) > 0.5)) { + || (abs(*right_drive_belt_velocity_) < 0.5 + && abs(*right_drive_belt_control_torque_) > 0.5)) { dirve_belt_block_count_++; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 981c6978..2c7c4299 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -17,10 +17,13 @@ class CatapultDart , private librmcs::client::CBoard { public: CatapultDart() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : 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()) - , dart_command_(create_partner_component(get_component_name() + "_command", *this)) + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) , dr16_(*this) , pitch_motor_( *this, *dart_command_, "/dart/pitch_motor", @@ -35,7 +38,9 @@ class CatapultDart {*this, *dart_command_, "/dart/drive_belt/left", device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, {*this, *dart_command_, "/dart/drive_belt/right", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}) + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(19.) + .set_reversed()}) , force_sensor_(*this) , trigger_servo_(*dart_command_, "/dart/trigger_servo") , transmit_buffer_(*this, 32) @@ -65,7 +70,11 @@ class CatapultDart void command_update() { uint16_t can_commands[4]; - transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); + if (pub_time_count_++ > 100) { + transmit_buffer_.add_can1_transmission( + 0x301, std::bit_cast(force_sensor_.generate_command())); + pub_time_count_ = 0; + } can_commands[0] = pitch_motor_.generate_command(); can_commands[1] = yaw_motor_.generate_command(); @@ -81,16 +90,20 @@ class CatapultDart if (!trigger_servo_.calibrate_mode()) { size_t uart_data_length; - std::unique_ptr command_buffer = trigger_servo_.generate_runtime_command(uart_data_length); + std::unique_ptr command_buffer = + trigger_servo_.generate_runtime_command(uart_data_length); const auto trigger_servo_uart_data_ptr = command_buffer.get(); transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); - std::string hex_string = bytes_to_hex_string(command_buffer.get(), uart_data_length); - RCLCPP_INFO(this->get_logger(), "UART2(length: %zu): [ %s ]", uart_data_length, hex_string.c_str()); + // std::string hex_string = bytes_to_hex_string(command_buffer.get(), uart_data_length); + // RCLCPP_INFO( + // this->get_logger(), "UART2(length: %zu): [ %s ]", uart_data_length, + // hex_string.c_str()); } transmit_buffer_.trigger_transmission(); } + int pub_time_count_ = 0; protected: void can1_receive_callback( @@ -118,9 +131,9 @@ class CatapultDart } else if (can_id == 0x203) { force_control_motor_.store_status(can_data); } else if (can_id == 0x205) { - drive_belt_motor_[0].generate_command(); + drive_belt_motor_[0].store_status(can_data); } else if (can_id == 0x206) { - drive_belt_motor_[1].generate_command(); + drive_belt_motor_[1].store_status(can_data); } } @@ -132,8 +145,9 @@ class CatapultDart RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); } - std::string hex_string = bytes_to_hex_string(data, length); - RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, hex_string.c_str()); + // std::string hex_string = bytes_to_hex_string(data, length); + // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, + // hex_string.c_str()); } void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { @@ -178,7 +192,9 @@ class CatapultDart transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); - RCLCPP_INFO(this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, hex_string.c_str()); + RCLCPP_INFO( + this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, + hex_string.c_str()); } static std::string bytes_to_hex_string(const std::byte* data, size_t size) { From 8188d4c38300e6013630c65b2b829e20d9de9953 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Wed, 3 Dec 2025 22:29:29 +0800 Subject: [PATCH 21/45] =?UTF-8?q?=E5=AE=8C=E6=88=90=E4=BA=86yaw=E4=B8=8Epi?= =?UTF-8?q?tch=E7=9A=84=E7=AE=80=E5=8D=95=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../rmcs_bringup/config/dart-pitch-pid.yaml | 55 ++++++ rmcs_ws/src/rmcs_core/plugins.xml | 11 ++ .../src/controller/dart/dart_setttings.cpp | 88 ++++++++- .../src/controller/dart/debug_info.cpp | 34 ++++ .../rmcs_core/src/hardware/catapult_dart.cpp | 177 +++++++++++++++--- .../src/hardware/device/force_sensor.hpp | 2 +- 6 files changed, 335 insertions(+), 32 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml new file mode 100644 index 00000000..db3dd966 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml @@ -0,0 +1,55 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDart -> catapult_dart + - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::controller::dart::Test -> actual_speed_and_control_torque_test_node + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> Value_Broadcasters + +catapult_dart: + ros__parameters: + usb_pid: -1 + imu_sensitivity: 2.0 + calibration_start_time: 5.0 + calibration_end_time: 10.0 + +dart_settings: + ros__parameters: + control_mode: 0 #<0 = manual_control_mode> or <1 = double_loop_pid> + pitch_angle_kp: 1.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + max_transform: 20.0 + +actual_speed_and_control_torque_test_node: + ros__parameters: {} + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /dart/pitch_motor/velocity + setpoint: /pitch/control/velocity + control: /dart/pitch_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /dart/yaw_motor/velocity + setpoint: /yaw/control/velocity + control: /dart/yaw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +Value_Broadcaster: + ros__parameters: + forward_list: + - "/dart/pitch/angle" + - "/dart/yaw/velocity" + - "/dart/pitch/velocity" + - "/yaw/control/velocity" + - "/pitch/control/velocity" \ 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..c85b6200 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -98,4 +98,15 @@ Test plugin. + + Test plugin. + + + Test plugin. + + + Test plugin. + + + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp index af5667e8..58f3d31a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp @@ -4,6 +4,8 @@ #include #include #include +#include "controller/pid/pid_calculator.hpp" +#include /* angle controls & launch parameter settings @@ -22,32 +24,112 @@ angle controls & launch parameter settings */ namespace rmcs_core::controller::dart { +enum class SwitchMode : uint8_t { + LOCKED =0, + UNLOCKED =1 +}; + class DartSettings : public rmcs_executor::Component , public rclcpp::Node { public: DartSettings() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) {} + , logger_(get_logger()) { + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/dart/pitch_motor/angle", pitch_angle_); + register_output("/yaw/control/velocity", yaw_control_velocity_); + register_output("/pitch/control/velocity", pitch_control_velocity_); + register_output("/pitch/angle/setpoint", pitch_angle_setpoint_); + direction_control_mode_ = get_parameter("control_mode").as_bool(); + kp_ = get_parameter("pitch_angle_kp").as_double(); + ki_ = get_parameter("pitch_angle_ki").as_double(); + kd_ = get_parameter("pitch_angle_kd").as_double(); + max_transform_ = get_parameter("max_transform").as_double(); + } + + void before_updating() override { + if (!switch_left_.ready()) { + RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_left\". Set to 0.0."); + } + if (!switch_right_.ready()) { + RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_right\". Set to 0.0."); + } + } + + void update() override { + using namespace rmcs_msgs; - void update() override {}; + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + do {if (direction_control_mode_ == 0) { // manual control mode + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + *yaw_control_velocity_ = joystick_left_->y() * max_transform_; + *pitch_control_velocity_ = joystick_right_->x() * max_transform_; + } else if (direction_control_mode_ == 1){ // double loop pid + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + *yaw_control_velocity_ = joystick_left_->y() * max_transform_; + pitch_control(); + } + } while(false); + + RCLCPP_INFO(this->get_logger(), "yaw_control_speed = %f, pitch_control_speed = %f", *yaw_control_velocity_, *pitch_control_velocity_); + + }; + + void pitch_control() { + *pitch_angle_setpoint_ = joystick_right_->x() * max_transform_; + pitch_angle_pid_controller.kp = kp_; + pitch_angle_pid_controller.ki = ki_; + pitch_angle_pid_controller.kd = kd_; + *pitch_control_velocity_ = pitch_angle_pid_controller.update(*pitch_angle_setpoint_ - *pitch_angle_); + } + + void reset_all_controls() { + *yaw_control_velocity_ = nan; + *pitch_control_velocity_ = nan; + } private: + static constexpr double nan = std::numeric_limits::quiet_NaN(); rclcpp::Logger logger_; + bool direction_control_mode_; + + pid::PidCalculator pitch_angle_pid_controller; + + double max_transform_; + double kp_, ki_, kd_; + InputInterface joystick_right_; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; + InputInterface pitch_angle_; + OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 + OutputInterface pitch_control_velocity_; OutputInterface force_setpoint_; + }; } // namespace rmcs_core::controller::dart #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp new file mode 100644 index 00000000..756edc3b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp @@ -0,0 +1,34 @@ +#include +#include +#include +#include + +namespace rmcs_core::controller::dart { +class Test + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Test() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + register_input("/dart/pitch_motor/control_torque", pitch_control_torque_); + register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); + register_input("/dart/pitch_motor/velocity", pitch_speed_); + register_input("/dart/yaw_motor/velocity", yaw_speed_); + } + void update() override { + // RCLCPP_INFO(this->get_logger(), "pitch_control_torque = %f, yaw_control_torque = %f", *pitch_control_torque_, *yaw_control_torque_); + // RCLCPP_INFO(this->get_logger(), "pitch_speed = %f, yaw_speed = %f", *pitch_speed_, *yaw_speed_); + } +private: + rclcpp::Logger logger_; + InputInterface pitch_control_torque_; + InputInterface yaw_control_torque_; + InputInterface pitch_speed_; + InputInterface yaw_speed_; +}; +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::Test, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 4571d601..71606af5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,11 +1,18 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" +#include "hardware/device/bmi088.hpp" #include "hardware/device/force_sensor.hpp" #include "librmcs/client/cboard.hpp" +#include #include +#include #include #include +#include "filter/low_pass_filter.hpp" +#include + namespace rmcs_core::hardware { + class CatapultDart : public rmcs_executor::Component , public rclcpp::Node @@ -15,23 +22,97 @@ class CatapultDart : 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()) + , pitch_angle_filter_(20.0, 1000.0) + , pitch_velocity_filter_(20.0, 1000.0) + , yaw_velocity_filter_(20.0, 1000.0) , dart_command_(create_partner_component(get_component_name() + "_command", *this)) , dr16_(*this) - , pitch_motor_(*this, *dart_command_, "/dart/pitch_motor") - , yaw_motor_(*this, *dart_command_, "/dart/yaw_motor") + , imu_(1000, 0.2, 0.0) + , pitch_motor_(*this, *dart_command_, "/dart/pitch_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19)) + , yaw_motor_(*this, *dart_command_, "/dart/yaw_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19)) , drive_belt_motor_( {*this, *dart_command_, "/dart/drive_belt/left"}, {*this, *dart_command_, "/dart/drive_belt/right"}) , force_control_motor_(*this, *dart_command_, "/dart/force_control_motor") , force_sensor_(*this) , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) {} + , event_thread_([this]() { handle_events(); }) { + + register_output("/dart/pitch/angle", pitch_angle_); + // register_output("/dart/pitch/velocity", pitch_velocity_output_); + // register_output("/dart/yaw/velocity", yaw_velocity_output_); + + imu_sensitivity_ = this->get_parameter("imu_sensitivity").as_double(); + calibration_start_time_ = this->get_parameter("calibration_start_time").as_double(); + calibration_end_time_ = this->get_parameter("calibration_end_time").as_double(); + + imu_.set_coordinate_mapping([](double x, double y, double z) { + return std::make_tuple(x, y, z); + }); + + start_time_ = std::chrono::steady_clock::now(); + calibration_complete_ = false; + } ~CatapultDart() override { stop_handling_events(); event_thread_.join(); } - void update() override {} + void update() override { + dr16_.update_status(); + imu_.update_status(); + + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); + + double raw_pitch_angle = extractPitchFromQuaternion( + imu_.q0() / imu_sensitivity_, + imu_.q1() / imu_sensitivity_, + imu_.q2() / imu_sensitivity_, + imu_.q3() / imu_sensitivity_ + ); + + double raw_pitch_velocity = imu_.gy(); + double raw_yaw_velocity = imu_.gz(); + + if (!calibration_complete_) { + if (elapsed_seconds >= calibration_start_time_ && elapsed_seconds <= calibration_end_time_) { + pitch_angle_sum_ += raw_pitch_angle; + pitch_velocity_sum_ += raw_pitch_velocity; + yaw_velocity_sum_ += raw_yaw_velocity; + sample_count_++; + } else if (elapsed_seconds > calibration_end_time_) { + if (sample_count_ > 0) { + pitch_angle_bias_ = pitch_angle_sum_ / sample_count_; + pitch_velocity_bias_ = pitch_velocity_sum_ / sample_count_; + yaw_velocity_bias_ = yaw_velocity_sum_ / sample_count_; + + RCLCPP_INFO(logger_, "Calibration complete: pitch_angle_bias=%.6f, pitch_vel_bias=%.6f, yaw_vel_bias=%.6f (samples=%zu)", + pitch_angle_bias_, pitch_velocity_bias_, yaw_velocity_bias_, sample_count_); + } + calibration_complete_ = true; + } + + *pitch_angle_ = 0.0; + *pitch_velocity_output_ = 0.0; + *yaw_velocity_output_ = 0.0; + } + else { + double corrected_pitch_angle = raw_pitch_angle - pitch_angle_bias_; + double corrected_pitch_velocity = raw_pitch_velocity - pitch_velocity_bias_; + double corrected_yaw_velocity = raw_yaw_velocity - yaw_velocity_bias_; + + *pitch_angle_ = pitch_angle_filter_.update(corrected_pitch_angle); + *pitch_velocity_output_ = pitch_velocity_filter_.update(corrected_pitch_velocity); + *yaw_velocity_output_ = yaw_velocity_filter_.update(corrected_yaw_velocity); + } + + pitch_motor_.update_status(); + yaw_motor_.update_status(); + + } void command_update() { uint16_t can_commands[4]; @@ -54,8 +135,9 @@ class CatapultDart can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - can_commands[0] = 0; - can_commands[1] = 0; + + can_commands[0] = pitch_motor_.generate_command(); + can_commands[1] = yaw_motor_.generate_command(); can_commands[2] = 0; can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); @@ -64,48 +146,87 @@ class CatapultDart } 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; - // } - - // 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; - - // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + 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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x201) { + pitch_motor_.store_status(can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(can_data); + + } + } - // void uart2_receive_callback(const std::byte* data, uint8_t length) override; + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); + } - // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } private: + static double extractPitchFromQuaternion(double q0, double q1, double q2, double q3) { + return std::asin(-2.0 * (q1 * q3 - q0 * q2)); + } + rclcpp::Logger logger_; + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + + bool calibration_complete_ = false; + std::chrono::steady_clock::time_point start_time_; + double calibration_start_time_; + double calibration_end_time_; + + double pitch_angle_sum_ = 0.0; + double pitch_velocity_sum_ = 0.0; + double yaw_velocity_sum_ = 0.0; + size_t sample_count_; + + double pitch_angle_bias_ = 0.0; + double pitch_velocity_bias_ = 0.0; + double yaw_velocity_bias_ = 0.0; + class DartCommand : public rmcs_executor::Component { public: - explicit DartCommand(CatapultDart& robot) - : dart(robot) {} + explicit DartCommand(CatapultDart& dart) + : dart_(dart) {} - void update() override { dart.command_update(); } + void update() override { dart_.command_update(); } - CatapultDart& dart; + CatapultDart& dart_; }; std::shared_ptr dart_command_; device::Dr16 dr16_; - + device::Bmi088 imu_; device::DjiMotor pitch_motor_; device::DjiMotor yaw_motor_; device::DjiMotor drive_belt_motor_[2]; device::DjiMotor force_control_motor_; - device::ForceSensor force_sensor_; librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; + + double imu_sensitivity_; + + OutputInterface pitch_angle_; + OutputInterface pitch_velocity_output_; + OutputInterface yaw_velocity_output_; }; -} // namespace rmcs_core::hardware \ No newline at end of file + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp index dc8a390e..53fccde9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -6,7 +6,7 @@ #include #include #include -#include +// #include namespace rmcs_core::hardware::device { From 9a6b2905dc3b71f8fc2858f98e137b7af2101fda Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Mon, 8 Dec 2025 19:42:41 +0800 Subject: [PATCH 22/45] Add device::ForceSensor and device::TriggerServo --- .../src/hardware/device/force_sensor.hpp | 89 ++++++++++ .../src/hardware/device/trigger_servo.hpp | 166 ++++++++++++++++++ 2 files changed, 255 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp new file mode 100644 index 00000000..74cd7226 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +/* +半双工通信设备，一发一收获取读数 +并不支持高频，10Hz下精确度较高且通信稳定 +*/ + +class ForceSensor { +public: + explicit ForceSensor(rmcs_executor::Component& status_component) { + status_component.register_output("/force_sensor/weight/channel1", weight_channel1_, nan_); + status_component.register_output("/force_sensor/weight/channel2", weight_channel2_, nan_); + } + + enum class Mode { ZERO_CALIBRATE, MEASUREMENT }; + + void store_status(const uint64_t can_data) { + force_sensor_status_.store( + std::bit_cast(can_data), std::memory_order::relaxed); + } + + void update_status() { + auto status = force_sensor_status_.load(std::memory_order::relaxed); + + uint32_t ch1_weight = static_cast(status.ch1_weight_0) << 24 + | static_cast(status.ch1_weight_1) << 16 + | static_cast(status.ch1_weight_2) << 8 + | static_cast(status.ch1_weight_3); + uint32_t ch2_weight = static_cast(status.ch2_weight_0) << 24 + | static_cast(status.ch2_weight_1) << 16 + | static_cast(status.ch2_weight_2) << 8 + | static_cast(status.ch2_weight_3); + + *weight_channel1_ = std::bit_cast(ch1_weight); + *weight_channel2_ = std::bit_cast(ch2_weight); + } + + static uint32_t get_can_id(Mode mode, uint32_t devive_id = 1) { + if (mode == Mode::MEASUREMENT) { + return 0x03 << 8 | devive_id; + } else if (mode == Mode::ZERO_CALIBRATE) { + return 0x02 << 8 | devive_id; + } else { + return 0; + } + } + static uint64_t generate_read_weight_command() { return 0x00; } + + static uint64_t generate_zero_calibration_command(int channel = 0) { + if (channel == 1) { + return 0x01; + } + if (channel == 2) { + return 0x02; + } else { + return 0x0F; + } + } + +private: + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + struct __attribute__((packed, aligned(8))) ForceSensorStatus { + uint8_t ch1_weight_0; + uint8_t ch1_weight_1; + uint8_t ch1_weight_2; + uint8_t ch1_weight_3; + uint8_t ch2_weight_0; + uint8_t ch2_weight_1; + uint8_t ch2_weight_2; + uint8_t ch2_weight_3; + }; + std::atomic force_sensor_status_{}; + static_assert(decltype(force_sensor_status_)::is_always_lock_free); + + rmcs_executor::Component::OutputInterface weight_channel1_; + rmcs_executor::Component::OutputInterface weight_channel2_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp new file mode 100644 index 00000000..215bb4a9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp @@ -0,0 +1,166 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class TriggerServo { +public: + TriggerServo(rmcs_executor::Component& command_component, const std::string& name) { + command_component.register_input(name + "/control_angle", control_angle_); + } + + enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, + }; + + bool calibrate_mode() const { return is_calibrate_mode_; } + + void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + + std::unique_ptr generate_runtime_command(size_t& output_length) { + size_t size = sizeof(WrightControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0x01; + command.data_length = 0x07; + command.command_type = 0x03; + command.command_address = 0x2A; + command.control_angle[0] = static_cast(*control_angle_ >> 8); + command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); + command.control_runtime[0] = 0x00; + command.control_runtime[1] = 0x00; + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + + return buffer; + } + + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + uint8_t* command = nullptr; + size_t command_size = 0; + + if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { + command_size = sizeof(switch_servo_mode_); + command = switch_servo_mode_; + } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { + command_size = sizeof(switch_motor_mode_); + command = switch_motor_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { + command_size = sizeof(switch_motor_forward_mode_); + command = switch_motor_forward_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { + command_size = sizeof(switch_motor_reverse_mode_); + command = switch_motor_reverse_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { + command_size = sizeof(motor_mode_runtime_control_); + command = motor_mode_runtime_control_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { + command_size = sizeof(motor_mode_disable_control_); + command = motor_mode_disable_control_; + } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { + command_size = sizeof(read_angle_command_); + command = read_angle_command_; + } + + std::unique_ptr buffer = std::make_unique(command_size); + std::memcpy(buffer.get(), command, command_size); + output_length = command_size; + + return buffer; + } + + static bool calibrate_current_angle( + const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != sizeof(ReadAngleReceiveData)) { + RCLCPP_INFO(logger, "UART data length not match"); + return false; + } + + ReadAngleReceiveData package; + uint8_t checksum; + + std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); + std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + + if (package.header[0] != 0xFF || package.header[1] != 0xF5) { + RCLCPP_INFO(logger, "UART data header not match"); + return false; + } + + if (checksum != package.calculate_checksum()) { + RCLCPP_INFO( + logger, "Checksum error: receive:%x,calc:%x", checksum, + package.calculate_checksum()); + return false; + } + + uint16_t current_angle = static_cast(package.current_angle[0]) << 8 + | static_cast(package.current_angle[1]); + RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); + return true; + } + +private: + struct __attribute__((packed)) WrightControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t control_angle[2]; + uint8_t control_runtime[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + + control_angle[1] + control_runtime[0] + control_runtime[1]; + return ~check; + } + }; + + struct __attribute__((packed)) ReadAngleReceiveData { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t servo_status; + uint8_t current_angle[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; + return ~check; + } + }; + uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02, 0xBE}; + + uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x01, 0xDA}; + uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x00, 0xDB}; + + uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x00, 0xDA}; + uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x01, 0xD9}; + + uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA1}; + uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB5}; + + rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + + std::atomic is_calibrate_mode_ = false; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file From 4f1b3ffe497a40a70eef5972aeb9bd50512bdc7b Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Mon, 8 Dec 2025 19:50:43 +0800 Subject: [PATCH 23/45] Pick basic controllers from dart branch --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 68 +++++ rmcs_ws/src/rmcs_core/plugins.xml | 9 + .../dartlauncher/dart_setttings.cpp | 109 +++++++ .../dartlauncher/launch_controller.cpp | 206 ++++++++++++++ .../rmcs_core/src/hardware/catapult_dart.cpp | 266 ++++++++++++++++++ .../include/rmcs_msgs/dart_launch_stage.hpp | 20 ++ 6 files changed, 678 insertions(+) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml new file mode 100644 index 00000000..6b8e986a --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -0,0 +1,68 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDart -> dart_hardware + - rmcs_core::controller::dart::DartLaunchController -> launch_controller + - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + +broadcaster: + ros__parameters: + forward_list: + - /dart/force_screw_motor/control_velocity + - /dart/force_screw_motor/velocity + - /dart/drive_belt/left/control_torque + - /dart/drive_belt/left/velocity + - /dart/drive_belt/right/control_torque + - /dart/drive_belt/right/velocity + +dart_hardware: + ros__parameters: + usb_pid: -1 + +launch_controller: + ros__parameters: + belt_velocity: 10.0 + sync_coefficient: 0.5 + b_kp: 1.0 + b_ki: 0.0 + b_kd: 0.0 + max_control_torque: 1.0 + trigger_free_angle: 0x0E2A + trigger_lock_angle: 0x0FC1 + +dart_settings: + ros__parameters: + log_enable: false + screw_max_velocity: 10.0 + +force_screw_velocity_pid_controller: + ros__parameters: + measurement: /dart/force_screw_motor/velocity + setpoint: /dart/force_screw_motor/control_velocity + control: /dart/force_screw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /dart/yaw_motor/velocity + setpoint: /dart/yaw_motor/control_velocity + control: /dart/yaw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /dart/pitch_motor/velocity + setpoint: /dart/pitch_motor/control_velocity + control: /dart/pitch_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 \ 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..7818b522 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -98,4 +98,13 @@ Test plugin. + + Test plugin. + + + Test plugin. + + + Test plugin. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp new file mode 100644 index 00000000..0a4913a3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp @@ -0,0 +1,109 @@ +#include +#include +#include +#include +#include +#include + +/* +angle controls & launch parameter settings +Version: 1.0.0 (manual control) +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +class DartSettings + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartSettings() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + log_enable_ = get_parameter("log_enable").as_bool(); + force_screw_max_velocity_ = get_parameter("screw_max_velocity").as_double(); + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/force_sensor/weight/channel1", force_sensor_ch1_weight_); + register_input("/force_sensor/weight/channel2", force_sensor_ch2_weight_); + + register_output("/dart/force_screw_motor/control_velocity", force_screw_control_velocity_); + register_output("/dart/yaw_motor/control_velocity", yaw_control_velocity_); + register_output("/dart/pitch_motor/control_velocity", pitch_control_velocity_); + } + + void update() override { + if (*switch_left_ == rmcs_msgs::Switch::UP) { + if (*switch_right_ == rmcs_msgs::Switch::DOWN) { + *force_screw_control_velocity_ = joystick_right_->x() * force_screw_max_velocity_; + } + if (*switch_right_ == rmcs_msgs::Switch::MIDDLE) { + *yaw_control_velocity_ = joystick_right_->y() * angle_control_max_velocity_; + *pitch_control_velocity_ = joystick_left_->x() * angle_control_max_velocity_; + } + } else { + reset(); + } + + if (log_enable_) { + force_sensor_measurement_log(); + } + } + +private: + void reset() { + *force_screw_control_velocity_ = 0; + *yaw_control_velocity_ = nan_; + *pitch_control_velocity_ = nan_; + } + + void force_sensor_measurement_log() { + if (log_count_++ > 100) { + log_count_ = 0; + RCLCPP_INFO( + logger_, "ch1: %d | ch2: %d", *force_sensor_ch1_weight_, *force_sensor_ch2_weight_); + } + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + + OutputInterface yaw_control_velocity_; + OutputInterface pitch_control_velocity_; + + InputInterface force_sensor_ch1_weight_, force_sensor_ch2_weight_; + OutputInterface force_screw_control_velocity_; + double force_screw_max_velocity_; + double angle_control_max_velocity_; + + bool log_enable_ = false; + int log_count_ = 0; +}; + +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp new file mode 100644 index 00000000..93bfc90d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp @@ -0,0 +1,206 @@ +#include "controller/pid/matrix_pid_calculator.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +class DartLaunchController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartLaunchController() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , drive_belt_pid_calculator_( + get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), + get_parameter("b_kd").as_double()) { + + dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + + launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); + launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + + register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); + register_output( + "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + + register_output("/dart/trigger_servo/control_angle", trigger_control_angle); + } + + void update() override { + using namespace rmcs_msgs; + + if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) + && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { + *launch_stage_ = DartLaunchStages::DISABLE; + reset_all_controls(); + + } else if (*switch_left_ == Switch::MIDDLE) { + + if (last_launch_stage_ == DartLaunchStages::DISABLE) { + *launch_stage_ = DartLaunchStages::RESETTING; + } + + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { + if (last_launch_stage_ == DartLaunchStages::INIT) { + *launch_stage_ = DartLaunchStages::LOADING; + } else if (last_launch_stage_ == DartLaunchStages::READY) { + *launch_stage_ = DartLaunchStages::CANCEL; + } + } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { + if (last_launch_stage_ == DartLaunchStages::READY) { + *launch_stage_ = DartLaunchStages::INIT; + trigger_lock_flag_ = false; + } else { + RCLCPP_INFO(logger_, "Dart has't been loaded !"); + } + } + + if (blocking_detection()) { + if (last_launch_stage_ == DartLaunchStages::LOADING) { + *launch_stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = true; + dirve_belt_block_count_ = 0; + + } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { + *launch_stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = false; + dirve_belt_block_count_ = 0; + + } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { + *launch_stage_ = + trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; + dirve_belt_block_count_ = 0; + } + } + double control_velocity = 0; + + if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { + control_velocity = -dirve_belt_working_velocity_; + } else if ( + *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING + || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { + control_velocity = dirve_belt_working_velocity_; + } else { + control_velocity = 0; + } + drive_belt_sync_control(control_velocity); + RCLCPP_INFO(logger_, "%lf", control_velocity); + } + + *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; + + last_switch_left_ = *switch_left_; + last_switch_right_ = *switch_right_; + last_launch_stage_ = *launch_stage_; + } + +private: + void reset_all_controls() { + *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + *left_drive_belt_control_torque_ = 0; + *right_drive_belt_control_torque_ = 0; + } + + void drive_belt_sync_control(double set_velocity) { + if (set_velocity == 0) { + *left_drive_belt_control_torque_ = 0; + *right_drive_belt_control_torque_ = 0; + return; + } + + Eigen::Vector2d setpoint_error{ + set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; + + Eigen::Vector2d relative_velocity{ + *left_drive_belt_velocity_ - *right_drive_belt_velocity_, + *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; + + Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + + *left_drive_belt_control_torque_ = + std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_drive_belt_control_torque_ = + std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + } + + bool blocking_detection() { + if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) + || (abs(*right_drive_belt_velocity_) < 0.5 + && abs(*right_drive_belt_control_torque_) > 0.5)) { + dirve_belt_block_count_++; + } + + return dirve_belt_block_count_ > 1000 ? true : false; + } + + int dirve_belt_block_count_ = 0; + double dirve_belt_working_velocity_; + + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + rmcs_msgs::Switch last_switch_right_; + rmcs_msgs::Switch last_switch_left_; + + OutputInterface left_drive_belt_control_torque_; + OutputInterface right_drive_belt_control_torque_; + InputInterface left_drive_belt_velocity_; + InputInterface right_drive_belt_velocity_; + + double max_control_torque_; + + OutputInterface launch_stage_; + rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + + uint16_t launch_lock_angle_; + uint16_t launch_trigger_angle_; + bool trigger_lock_flag_ = false; + OutputInterface trigger_control_angle; + + pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; + double sync_coefficient_; +}; + +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp new file mode 100644 index 00000000..13eeac8b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -0,0 +1,266 @@ +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/force_sensor.hpp" +#include "hardware/device/trigger_servo.hpp" +#include "librmcs/client/cboard.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { +class CatapultDart + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + CatapultDart() + : 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()) + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , dr16_(*this) + , pitch_motor_( + *this, *dart_command_, "/dart/pitch_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) + , yaw_motor_( + *this, *dart_command_, "/dart/yaw_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) + , force_control_motor_( + *this, *dart_command_, "/dart/force_screw_motor", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) + , drive_belt_motor_( + {*this, *dart_command_, "/dart/drive_belt/left", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, + {*this, *dart_command_, "/dart/drive_belt/right", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(19.) + .set_reversed()}) + , force_sensor_(*this) + , trigger_servo_(*dart_command_, "/dart/trigger_servo") + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + + trigger_calibrate_subscription_ = create_subscription( + "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_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; + }; + } + + ~CatapultDart() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_[0].update_status(); + drive_belt_motor_[1].update_status(); + force_control_motor_.update_status(); + force_sensor_.update_status(); + } + + void command_update() { + uint16_t can_commands[4]; + + if (force_sensor_pub_time_count_++ > 100) { + force_sensor_pub_time_count_ = 0; + + transmit_buffer_.add_can1_transmission( + force_sensor_.get_can_id(device::ForceSensor::Mode::MEASUREMENT, 1), + force_sensor_.generate_read_weight_command()); + } + + can_commands[0] = pitch_motor_.generate_command(); + can_commands[1] = yaw_motor_.generate_command(); + can_commands[2] = force_control_motor_.generate_command(); + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + + can_commands[0] = drive_belt_motor_[0].generate_command(); + can_commands[1] = drive_belt_motor_[1].generate_command(); + can_commands[2] = 0; + can_commands[3] = 0; + transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + + if (!trigger_servo_.calibrate_mode()) { + size_t uart_data_length; + std::unique_ptr command_buffer = + trigger_servo_.generate_runtime_command(uart_data_length); + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); + } + + transmit_buffer_.trigger_transmission(); + } + +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 == 0x302) { + force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x201) { + pitch_motor_.store_status(can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(can_data); + } else if (can_id == 0x203) { + force_control_motor_.store_status(can_data); + } else if (can_id == 0x205) { + drive_belt_motor_[0].store_status(can_data); + } else if (can_id == 0x206) { + drive_belt_motor_[1].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 uart2_receive_callback(const std::byte* data, uint8_t length) override { + bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); + if (!success) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + + // std::string hex_string = bytes_to_hex_string(data, length); + // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, + // hex_string.c_str()); + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + +private: + void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { + /* + 标定命令格式： + ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" + 替换data值就行 + */ + trigger_servo_.set_calibrate_mode(msg->data); + + std::unique_ptr command_buffer; + size_t command_length = 0; + if (msg->data == 0) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); + } else if (msg->data == 1) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); + } else if (msg->data == 2) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); + } else if (msg->data == 3) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); + } else if (msg->data == 4) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); + } else if (msg->data == 5) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); + } else if (msg->data == 6) { + command_buffer = trigger_servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); + } + + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); + + std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); + RCLCPP_INFO( + this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, + hex_string.c_str()); + } + + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) { + return "[]"; + } + + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + + for (size_t i = 0; i < size; ++i) { + ss << std::setw(2) << static_cast(data[i]) << " "; + } + + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') { + result.pop_back(); + } + return result; + } // DEBUG TOOL + + rclcpp::Logger logger_; + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDart& robot) + : dart_(robot) {} + + void update() override { dart_.command_update(); } + + CatapultDart& dart_; + }; + std::shared_ptr dart_command_; + + device::Dr16 dr16_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor force_control_motor_; + device::DjiMotor drive_belt_motor_[2]; + + device::ForceSensor force_sensor_; + int force_sensor_pub_time_count_ = 0; // ForceSensor仅支持10Hz + + device::TriggerServo trigger_servo_; + + rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; + + 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::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp new file mode 100644 index 00000000..9ce532b9 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp @@ -0,0 +1,20 @@ +namespace rmcs_msgs { +enum class DartLaunchStages { + DISABLE, + INIT, + LOADING, + RESETTING, + READY, + CANCEL, +}; +/* +说明： + DISABLE：电机全部失能，遥控器双下 + INIT：初始状态，需要滑台复位 + LOADING：上膛和填装中 + RESETTING: 同步带复位中 + READY：可发射状态。发射完成后进入INIT +特殊： + READY -> INIT：退膛，转动带下去接滑台使其复位，避免空放 +*/ +} // namespace rmcs_msgs \ No newline at end of file From 99372c312674a8d83e165432040827c6ef7267ad Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Thu, 11 Dec 2025 05:41:02 +0800 Subject: [PATCH 24/45] Add torque limit for resetting stage --- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 10 +++++----- .../controller/dartlauncher/launch_controller.cpp | 13 ++++++++----- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index 6b8e986a..3b724233 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -27,17 +27,17 @@ dart_hardware: launch_controller: ros__parameters: belt_velocity: 10.0 - sync_coefficient: 0.5 + sync_coefficient: 0.1 b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 - max_control_torque: 1.0 + max_control_torque: 15.0 trigger_free_angle: 0x0E2A trigger_lock_angle: 0x0FC1 dart_settings: ros__parameters: - log_enable: false + log_enable: true screw_max_velocity: 10.0 force_screw_velocity_pid_controller: @@ -54,7 +54,7 @@ yaw_velocity_pid_controller: measurement: /dart/yaw_motor/velocity setpoint: /dart/yaw_motor/control_velocity control: /dart/yaw_motor/control_torque - kp: 1.0 + kp: 0.0 ki: 0.0 kd: 0.0 @@ -63,6 +63,6 @@ pitch_velocity_pid_controller: measurement: /dart/pitch_motor/velocity setpoint: /dart/pitch_motor/control_velocity control: /dart/pitch_motor/control_torque - kp: 1.0 + kp: 0.0 ki: 0.0 kd: 0.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp index 93bfc90d..3a068201 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp @@ -108,18 +108,21 @@ class DartLaunchController } } double control_velocity = 0; + double control_torque_limit = max_control_torque_; if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { control_velocity = -dirve_belt_working_velocity_; + control_torque_limit = max_control_torque_ * 0.1; } else if ( *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { control_velocity = dirve_belt_working_velocity_; + control_torque_limit = max_control_torque_; } else { control_velocity = 0; } - drive_belt_sync_control(control_velocity); - RCLCPP_INFO(logger_, "%lf", control_velocity); + drive_belt_sync_control(control_velocity, control_torque_limit); + // RCLCPP_INFO(logger_, "%lf", control_velocity); } *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; @@ -136,7 +139,7 @@ class DartLaunchController *right_drive_belt_control_torque_ = 0; } - void drive_belt_sync_control(double set_velocity) { + void drive_belt_sync_control(double set_velocity, double control_torque_limit) { if (set_velocity == 0) { *left_drive_belt_control_torque_ = 0; *right_drive_belt_control_torque_ = 0; @@ -153,9 +156,9 @@ class DartLaunchController Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + std::clamp(control_torques[0], -control_torque_limit, control_torque_limit); *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + std::clamp(control_torques[1], -control_torque_limit, control_torque_limit); } bool blocking_detection() { From 57efe926aad0c5e833c9b2761c4a56f7d755f798 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Thu, 11 Dec 2025 05:41:42 +0800 Subject: [PATCH 25/45] Add force sensor calibrate --- .../rmcs_core/src/hardware/catapult_dart.cpp | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 13eeac8b..18c101f9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -47,6 +47,12 @@ class CatapultDart , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { + force_sensor_calibrate_subscription_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + force_sensor_calibrate_subscription_callback(std::move(msg)); + }); + trigger_calibrate_subscription_ = create_subscription( "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { trigger_servo_calibrate_subscription_callback(std::move(msg)); @@ -149,22 +155,29 @@ class CatapultDart [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); } - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); - if (!success) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } + // void uart2_receive_callback(const std::byte* data, uint8_t length) override { + // bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); + // if (!success) { + // RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + // } - // std::string hex_string = bytes_to_hex_string(data, length); - // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, - // hex_string.c_str()); - } + // // std::string hex_string = bytes_to_hex_string(data, length); + // // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, + // // hex_string.c_str()); + // } void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { dr16_.store_status(uart_data, uart_data_length); } private: + void force_sensor_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + transmit_buffer_.add_can1_transmission( + force_sensor_.get_can_id(device::ForceSensor::Mode::ZERO_CALIBRATE, 1), + force_sensor_.generate_zero_calibration_command()); + RCLCPP_INFO(logger_, "call"); + } + void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { /* 标定命令格式： @@ -252,6 +265,7 @@ class CatapultDart device::TriggerServo trigger_servo_; rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; + rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; OutputInterface referee_serial_; From b7349c1eb92e9d2f1904c07b8ea89028ed3c3b39 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Sun, 14 Dec 2025 21:56:58 +0800 Subject: [PATCH 26/45] sensor loop to be done --- .vscode/settings.default.json | 8 +- .../rmcs_bringup/config/dart-pitch-pid.yaml | 44 +- rmcs_ws/src/rmcs_core/plugins.xml | 6 - .../src/controller/dart/dart_settings.cpp | 179 ++++++++ .../src/controller/dart/dart_setttings.cpp | 191 -------- .../rmcs_core/src/hardware/catapult_dart.cpp | 423 +++++++++++------- 6 files changed, 485 insertions(+), 366 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp diff --git a/.vscode/settings.default.json b/.vscode/settings.default.json index b1a1eaf0..b04a69f8 100644 --- a/.vscode/settings.default.json +++ b/.vscode/settings.default.json @@ -3,12 +3,12 @@ "--header-insertion=never" ], "python.autoComplete.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" + "/opt/ros/jazzy/lib/python3.10/site-packages", + "/opt/ros/jazzy/local/lib/python3.10/dist-packages" ], "python.analysis.extraPaths": [ - "/opt/ros/humble/lib/python3.10/site-packages", - "/opt/ros/humble/local/lib/python3.10/dist-packages" + "/opt/ros/jazzy/lib/python3.10/site-packages", + "/opt/ros/jazzy/local/lib/python3.10/dist-packages" ], "cSpell.words": [ "ahrs", diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml index db3dd966..6f9833ad 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml @@ -4,25 +4,42 @@ rmcs_executor: components: - rmcs_core::hardware::CatapultDart -> catapult_dart - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::controller::dart::DartLaunchController -> launch_controller - rmcs_core::controller::dart::Test -> actual_speed_and_control_torque_test_node - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> force_pid_controller - rmcs_core::broadcaster::ValueBroadcaster -> Value_Broadcasters catapult_dart: ros__parameters: usb_pid: -1 imu_sensitivity: 2.0 - calibration_start_time: 5.0 - calibration_end_time: 10.0 + first_sample_spot: 5.0 + final_sample_spot: 10.0 dart_settings: ros__parameters: - control_mode: 0 #<0 = manual_control_mode> or <1 = double_loop_pid> + is_auto_pitch_control_mode: 0 #<0 = false = manual_control_mode> or <1 = true = double_loop_pid> + is_auto_force_control_mode: 0 #<0 = false = manual_control_mode> or <1 = true = force_sensor> pitch_angle_kp: 1.0 pitch_angle_ki: 0.0 pitch_angle_kd: 0.0 - max_transform: 20.0 + force_control_kp: 0.5 + force_control_ki: 0.0 + force_control_kd: 0.0 + max_transform_rate: 20.0 + +launch_controller: + ros__parameters: + belt_velocity: 10.0 + sync_coefficient: 0.5 + b_kp: 1.0 + b_ki: 0.0 + b_kd: 0.0 + max_control_torque: 2.5 + trigger_free_angle: 0x0E2A + trigger_lock_angle: 0x0FC1 actual_speed_and_control_torque_test_node: ros__parameters: {} @@ -45,11 +62,20 @@ yaw_velocity_pid_controller: ki: 0.0 kd: 0.0 -Value_Broadcaster: +force_pid_controller: + ros__parameters: + measurement: /dart/force_control_motor/velocity + setpoint: /force/control/velocity + control: /dart/force_control_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +Value_Broadcasters: ros__parameters: forward_list: - "/dart/pitch/angle" - - "/dart/yaw/velocity" - - "/dart/pitch/velocity" - - "/yaw/control/velocity" - - "/pitch/control/velocity" \ No newline at end of file + - "/dart/force_control_motor/velocity" + - "/imu/state/final_yaw" + - "/imu/state/final_pitch" + - "/imu/state/final_roll" \ 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 d28ea796..3113c573 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -98,8 +98,6 @@ Test plugin. -<<<<<<< HEAD -======= Test plugin. @@ -109,22 +107,18 @@ Test plugin. ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e Test plugin. Test plugin. -<<<<<<< HEAD Test plugin. -======= Test plugin. ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp new file mode 100644 index 00000000..c011551f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp @@ -0,0 +1,179 @@ +#include +#include +#include +#include +#include +#include +#include "controller/pid/pid_calculator.hpp" +#include + +/* +angle controls & launch parameter settings +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +enum class SwitchMode : uint8_t { + LOCKED =0, + UNLOCKED =1 +}; + +class DartSettings + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartSettings() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/dart/pitch_motor/angle", pitch_angle_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); + register_output("/yaw/control/velocity", yaw_control_velocity_); + register_output("/force/control/velocity", force_control_); + register_output("/pitch/control/velocity", pitch_control_velocity_); + register_output("/force/sensor/average", average_force_); + is_auto_pitch_control_mode_ = get_parameter("is_auto_pitch_control_mode").as_int(); + is_auto_force_control_mode_ = get_parameter("is_auto_force_control_mode").as_int(); + pitch_angle_kp_ = get_parameter("pitch_angle_kp").as_double(); + pitch_angle_ki_ = get_parameter("pitch_angle_ki").as_double(); + pitch_angle_kd_ = get_parameter("pitch_angle_kd").as_double(); + force_control_kp_ = get_parameter("force_control_kp").as_double(); + force_control_ki_ = get_parameter("force_control_ki").as_double(); + force_control_kd_ = get_parameter("force_control_kd").as_double(); + max_transform_rate_ = get_parameter("max_transform_rate").as_double(); + } + + void before_updating() override { + if (!switch_left_.ready()) { + RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_left\". Set to 0.0."); + } + if (!switch_right_.ready()) { + RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_right\". Set to 0.0."); + } + } + + void update() override { + using namespace rmcs_msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + do {if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::UP && switch_right == Switch::MIDDLE)) { + if (is_auto_pitch_control_mode_ == 0) { // manual control mode + *yaw_control_velocity_ = joystick_left_->y() * max_transform_rate_; + *pitch_control_velocity_ = joystick_right_->x() * max_transform_rate_; + break; + } else if (is_auto_pitch_control_mode_ == 1){ // double loop pid + *yaw_control_velocity_ = joystick_left_->y() * max_transform_rate_; + pitch_control(); + break; + } + } + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::UP && switch_right == Switch::DOWN)) { + if (is_auto_force_control_mode_ == 0) { + *force_control_ = joystick_right_->x() * max_transform_rate_ * 5; + break; + } else if (is_auto_force_control_mode_ == 1) { + *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) / 2; + transmit_distance_to_force(); + force_control(); + log_count_++; + if (log_count_ == 100) { + log_count_ = 0; + RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); + } + } + } + } while(false); + + // RCLCPP_INFO(this->get_logger(), "yaw_control_speed = %f, pitch_control_speed = %f, force_control_speed = %f", *yaw_control_velocity_, *pitch_control_velocity_, *force_control_); + + }; + +private: + void reset_all_controls() { + *yaw_control_velocity_ = nan; + *pitch_control_velocity_ = nan; + *force_control_ = nan; + } + + void pitch_control() { + pitch_angle_setpoint_ = joystick_right_->x() * max_transform_rate_; + pitch_angle_pid_controller.kp = pitch_angle_kp_; + pitch_angle_pid_controller.ki = pitch_angle_ki_; + pitch_angle_pid_controller.kd = pitch_angle_kd_; + *pitch_control_velocity_ = pitch_angle_pid_controller.update(pitch_angle_setpoint_ - *pitch_angle_); + } + + void force_control() { + force_control_pid_controller.kp = force_control_kp_; + force_control_pid_controller.ki = force_control_ki_; + force_control_pid_controller.kd = force_control_kd_; + *force_control_ = force_control_pid_controller.update(force_control_setpoint_ - *average_force_); + } + + void transmit_distance_to_force() { + force_control_setpoint_ = 3800.0; + } + +private: + static constexpr double nan = std::numeric_limits::quiet_NaN(); + rclcpp::Logger logger_; + + int log_count_ = 0; + int is_auto_pitch_control_mode_; + int is_auto_force_control_mode_; + double max_transform_rate_; + + double pitch_angle_setpoint_; + double force_control_setpoint_; + + pid::PidCalculator pitch_angle_pid_controller; + pid::PidCalculator force_control_pid_controller; + + double pitch_angle_kp_, pitch_angle_ki_, pitch_angle_kd_; + double force_control_kp_, force_control_ki_, force_control_kd_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + + InputInterface pitch_angle_; + InputInterface force_sensor_ch1_data_; + InputInterface force_sensor_ch2_data_; + + OutputInterface yaw_control_velocity_; + OutputInterface force_control_; + OutputInterface pitch_control_velocity_; + OutputInterface average_force_; + +}; + +} // namespace rmcs_core::controller::dart + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp deleted file mode 100644 index 270761bc..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setttings.cpp +++ /dev/null @@ -1,191 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "controller/pid/pid_calculator.hpp" -#include - -/* -angle controls & launch parameter settings -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -enum class SwitchMode : uint8_t { - LOCKED =0, - UNLOCKED =1 -}; - -class DartSettings - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartSettings() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { -<<<<<<< HEAD - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/dart/pitch_motor/angle", pitch_angle_); - register_output("/yaw/control/velocity", yaw_control_velocity_); - register_output("/pitch/control/velocity", pitch_control_velocity_); - register_output("/pitch/angle/setpoint", pitch_angle_setpoint_); - direction_control_mode_ = get_parameter("control_mode").as_bool(); - kp_ = get_parameter("pitch_angle_kp").as_double(); - ki_ = get_parameter("pitch_angle_ki").as_double(); - kd_ = get_parameter("pitch_angle_kd").as_double(); - max_transform_ = get_parameter("max_transform").as_double(); - } - - void before_updating() override { - if (!switch_left_.ready()) { - RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_left\". Set to 0.0."); - } - if (!switch_right_.ready()) { - RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_right\". Set to 0.0."); - } - } - - void update() override { - using namespace rmcs_msgs; - - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - - do {if (direction_control_mode_ == 0) { // manual control mode - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - *yaw_control_velocity_ = joystick_left_->y() * max_transform_; - *pitch_control_velocity_ = joystick_right_->x() * max_transform_; - } else if (direction_control_mode_ == 1){ // double loop pid - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - *yaw_control_velocity_ = joystick_left_->y() * max_transform_; - pitch_control(); - } - } while(false); - - RCLCPP_INFO(this->get_logger(), "yaw_control_speed = %f, pitch_control_speed = %f", *yaw_control_velocity_, *pitch_control_velocity_); - - }; - - void pitch_control() { - *pitch_angle_setpoint_ = joystick_right_->x() * max_transform_; - pitch_angle_pid_controller.kp = kp_; - pitch_angle_pid_controller.ki = ki_; - pitch_angle_pid_controller.kd = kd_; - *pitch_control_velocity_ = pitch_angle_pid_controller.update(*pitch_angle_setpoint_ - *pitch_angle_); - } - - void reset_all_controls() { - *yaw_control_velocity_ = nan; - *pitch_control_velocity_ = nan; - } - -private: - static constexpr double nan = std::numeric_limits::quiet_NaN(); -======= - log_enable_ = get_parameter("log_enable").as_bool(); - force_screw_max_velocity_ = get_parameter("screw_max_velocity").as_double(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_weight_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_weight_); - - register_output("/dart/force_control_motor/control_velocity", force_control_velocity_); - } - - void update() override { - if (*switch_left_ == rmcs_msgs::Switch::UP) { - if (*switch_right_ == rmcs_msgs::Switch::DOWN) { - *force_control_velocity_ = joystick_right_->x() * force_screw_max_velocity_; - } - if (*switch_right_ == rmcs_msgs::Switch::MIDDLE) { - // angle control - } - } else { - reset(); - } - - if (log_enable_) { - measure_log(); - } - } - -private: - void reset() { *force_control_velocity_ = 0; } - - void measure_log() { - if (log_count_++ > 100) { - log_count_ = 0; - RCLCPP_INFO(logger_, "ch1: %d | ch2: %d", *force_sensor_ch1_weight_, *force_sensor_ch2_weight_); - } - } - ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e - rclcpp::Logger logger_; - - bool direction_control_mode_; - - pid::PidCalculator pitch_angle_pid_controller; - - double max_transform_; - double kp_, ki_, kd_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - -<<<<<<< HEAD - InputInterface pitch_angle_; - - OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 - OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 - OutputInterface pitch_control_velocity_; - - OutputInterface force_setpoint_; - -======= - // OutputInterface yaw_control_velocity_; // 只需要速度环，摇杆直接映射成速度 - // OutputInterface pitch_angle_setpoint_; // 双环pid，外环角度内环速度，摇杆映射成目标角度 - - InputInterface force_sensor_ch1_weight_, force_sensor_ch2_weight_; - OutputInterface force_control_velocity_; - double force_screw_max_velocity_; - - bool log_enable_ = false; - int log_count_ = 0; ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 6284de16..ef44e4e8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,29 +1,55 @@ #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" -<<<<<<< HEAD #include "hardware/device/bmi088.hpp" #include "hardware/device/force_sensor.hpp" #include "librmcs/client/cboard.hpp" #include -======= #include "hardware/device/force_sensor_runtime.hpp" #include "hardware/device/trigger_servo.hpp" -#include "librmcs/client/cboard.hpp" #include ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e #include #include #include #include -<<<<<<< HEAD #include "filter/low_pass_filter.hpp" #include -======= #include ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e +#include +#include +#include +#include +#include namespace rmcs_core::hardware { +class QuaternionBayesFilter { +public: + QuaternionBayesFilter(double confidence_ratio, double sample_rate) + : alpha_(confidence_ratio / (confidence_ratio + 1.0)) + , initialized_(false) {} + + Eigen::Quaterniond update(const Eigen::Quaterniond& measurement) { + if (!initialized_) { + belief_ = measurement; + initialized_ = true; + return belief_; + } + + belief_ = belief_.slerp(1.0 - alpha_, measurement); + return belief_; + } + + void reset() { initialized_ = false; } + bool isInitialized() const { return initialized_; } + double getConfidence() const { return alpha_; } + Eigen::Quaterniond getCurrentBelief() const { return belief_; } + +private: + double alpha_; + bool initialized_; + Eigen::Quaterniond belief_; +}; + class CatapultDart : public rmcs_executor::Component , public rclcpp::Node @@ -35,21 +61,12 @@ class CatapultDart rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} , logger_(get_logger()) -<<<<<<< HEAD , pitch_angle_filter_(20.0, 1000.0) , pitch_velocity_filter_(20.0, 1000.0) , yaw_velocity_filter_(20.0, 1000.0) , dart_command_(create_partner_component(get_component_name() + "_command", *this)) , dr16_(*this) , imu_(1000, 0.2, 0.0) - , pitch_motor_(*this, *dart_command_, "/dart/pitch_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19)) - , yaw_motor_(*this, *dart_command_, "/dart/yaw_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19)) -======= - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , dr16_(*this) , pitch_motor_( *this, *dart_command_, "/dart/pitch_motor", device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) @@ -59,7 +76,6 @@ class CatapultDart , force_control_motor_( *this, *dart_command_, "/dart/force_control_motor", device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e , drive_belt_motor_( {*this, *dart_command_, "/dart/drive_belt/left", device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, @@ -71,29 +87,26 @@ class CatapultDart , trigger_servo_(*dart_command_, "/dart/trigger_servo") , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { -<<<<<<< HEAD register_output("/dart/pitch/angle", pitch_angle_); - // register_output("/dart/pitch/velocity", pitch_velocity_output_); - // register_output("/dart/yaw/velocity", yaw_velocity_output_); - + register_output("/tf", tf_); + register_output("/imu/state/final_roll", final_roll); + register_output("/imu/state/final_pitch", final_pitch); + register_output("/imu/state/final_yaw", final_yaw); + imu_sensitivity_ = this->get_parameter("imu_sensitivity").as_double(); - calibration_start_time_ = this->get_parameter("calibration_start_time").as_double(); - calibration_end_time_ = this->get_parameter("calibration_end_time").as_double(); - - imu_.set_coordinate_mapping([](double x, double y, double z) { - return std::make_tuple(x, y, z); - }); + first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); + final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + imu_sampler_initialize(); + tf_broadcaster_ = std::make_unique(*this); start_time_ = std::chrono::steady_clock::now(); calibration_complete_ = false; -======= trigger_calibrate_subscription_ = create_subscription( "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { trigger_servo_calibrate_subscription_callback(std::move(msg)); }); ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e } ~CatapultDart() override { @@ -103,65 +116,16 @@ class CatapultDart void update() override { dr16_.update_status(); -<<<<<<< HEAD - imu_.update_status(); - - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); - - double raw_pitch_angle = extractPitchFromQuaternion( - imu_.q0() / imu_sensitivity_, - imu_.q1() / imu_sensitivity_, - imu_.q2() / imu_sensitivity_, - imu_.q3() / imu_sensitivity_ - ); - - double raw_pitch_velocity = imu_.gy(); - double raw_yaw_velocity = imu_.gz(); - - if (!calibration_complete_) { - if (elapsed_seconds >= calibration_start_time_ && elapsed_seconds <= calibration_end_time_) { - pitch_angle_sum_ += raw_pitch_angle; - pitch_velocity_sum_ += raw_pitch_velocity; - yaw_velocity_sum_ += raw_yaw_velocity; - sample_count_++; - } else if (elapsed_seconds > calibration_end_time_) { - if (sample_count_ > 0) { - pitch_angle_bias_ = pitch_angle_sum_ / sample_count_; - pitch_velocity_bias_ = pitch_velocity_sum_ / sample_count_; - yaw_velocity_bias_ = yaw_velocity_sum_ / sample_count_; - - RCLCPP_INFO(logger_, "Calibration complete: pitch_angle_bias=%.6f, pitch_vel_bias=%.6f, yaw_vel_bias=%.6f (samples=%zu)", - pitch_angle_bias_, pitch_velocity_bias_, yaw_velocity_bias_, sample_count_); - } - calibration_complete_ = true; - } - - *pitch_angle_ = 0.0; - *pitch_velocity_output_ = 0.0; - *yaw_velocity_output_ = 0.0; - } - else { - double corrected_pitch_angle = raw_pitch_angle - pitch_angle_bias_; - double corrected_pitch_velocity = raw_pitch_velocity - pitch_velocity_bias_; - double corrected_yaw_velocity = raw_yaw_velocity - yaw_velocity_bias_; - - *pitch_angle_ = pitch_angle_filter_.update(corrected_pitch_angle); - *pitch_velocity_output_ = pitch_velocity_filter_.update(corrected_pitch_velocity); - *yaw_velocity_output_ = yaw_velocity_filter_.update(corrected_yaw_velocity); - } - - pitch_motor_.update_status(); - yaw_motor_.update_status(); - -======= + pitch_motor_.update_status(); yaw_motor_.update_status(); drive_belt_motor_[0].update_status(); drive_belt_motor_[1].update_status(); force_control_motor_.update_status(); force_sensor_.update_status(); ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e + + imu_.update_status(); + processImuData(); } void command_update() { @@ -173,28 +137,9 @@ class CatapultDart pub_time_count_ = 0; } -<<<<<<< HEAD - can_commands[0] = 0; - can_commands[1] = 0; - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = 0; - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - - - can_commands[0] = pitch_motor_.generate_command(); - can_commands[1] = yaw_motor_.generate_command(); - can_commands[2] = 0; -======= can_commands[0] = pitch_motor_.generate_command(); can_commands[1] = yaw_motor_.generate_command(); can_commands[2] = force_control_motor_.generate_command(); ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e can_commands[3] = 0; transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); @@ -222,23 +167,12 @@ class CatapultDart int pub_time_count_ = 0; protected: -<<<<<<< HEAD - void can2_receive_callback( -======= void can1_receive_callback( ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e 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; -<<<<<<< HEAD - if (can_id == 0x201) { - pitch_motor_.store_status(can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(can_data); - -======= if (can_id == 0x302) { force_sensor_.store_status(can_data); @@ -261,22 +195,23 @@ class CatapultDart drive_belt_motor_[0].store_status(can_data); } else if (can_id == 0x206) { drive_belt_motor_[1].store_status(can_data); ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e } } void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + x = static_cast(x / imu_sensitivity_); + y = static_cast(y / imu_sensitivity_); + z = static_cast(z / imu_sensitivity_); imu_.store_accelerometer_status(x, y, z); } void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + x = static_cast(x / imu_sensitivity_); + y = static_cast(y / imu_sensitivity_); + z = static_cast(z / imu_sensitivity_); imu_.store_gyroscope_status(x, y, z); } -<<<<<<< HEAD - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); -======= void uart2_receive_callback(const std::byte* data, uint8_t length) override { bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); if (!success) { @@ -291,14 +226,8 @@ class CatapultDart void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { dr16_.store_status(uart_data, uart_data_length); } ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e - } private: -<<<<<<< HEAD - static double extractPitchFromQuaternion(double q0, double q1, double q2, double q3) { - return std::asin(-2.0 * (q1 * q3 - q0 * q2)); -======= void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { /* 标定命令格式： @@ -358,7 +287,6 @@ class CatapultDart result.pop_back(); } return result; ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e } rclcpp::Logger logger_; @@ -368,28 +296,13 @@ class CatapultDart filter::LowPassFilter<1> yaw_velocity_filter_; bool calibration_complete_ = false; - std::chrono::steady_clock::time_point start_time_; - double calibration_start_time_; - double calibration_end_time_; - - double pitch_angle_sum_ = 0.0; - double pitch_velocity_sum_ = 0.0; - double yaw_velocity_sum_ = 0.0; - size_t sample_count_; - - double pitch_angle_bias_ = 0.0; - double pitch_velocity_bias_ = 0.0; - double yaw_velocity_bias_ = 0.0; + std::chrono::steady_clock::time_point start_time_; + class DartCommand : public rmcs_executor::Component { public: -<<<<<<< HEAD - explicit DartCommand(CatapultDart& dart) - : dart_(dart) {} -======= explicit DartCommand(CatapultDart& robot) : dart_(robot) {} ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e void update() override { dart_.command_update(); } @@ -397,40 +310,238 @@ class CatapultDart }; std::shared_ptr dart_command_; + void imu_sampler_initialize() { + start_time_ = std::chrono::steady_clock::now(); + calibration_complete_ = false; + sample_counter_ = 0; + quaternion_filter_.reset(); + quaternion_smoother_.reset(); + clearSamples(); + } + + void processImuData() { + Eigen::Quaterniond raw_quaternion{ + imu_.q0() / imu_sensitivity_, + imu_.q1() / imu_sensitivity_, + imu_.q2() / imu_sensitivity_, + imu_.q3() / imu_sensitivity_ + }; + raw_quaternion.normalize(); + + Eigen::Quaterniond bayes_quaternion = quaternion_filter_.update(raw_quaternion); + + Eigen::Quaterniond smoothed_quaternion = quaternion_smoother_.update(bayes_quaternion); + + Eigen::Vector3d smoothed_euler = quaternionToEuler(smoothed_quaternion); + + auto compensated = applyDriftCompensation(smoothed_euler); + + setOutputStates(compensated); + + setTfTransforms(compensated); + } + + struct CompensatedResult { double roll, pitch, yaw; }; + + CompensatedResult applyDriftCompensation(const Eigen::Vector3d& angles) { + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); + + if (!calibration_complete_) { + collectCalibrationSamples(angles, elapsed_seconds); + return {angles.x(), angles.y(), angles.z()}; + } else { + double compensated_roll = angles.x() - roll_bias_; + double compensated_pitch = angles.y() - pitch_bias_; + double compensated_yaw = angles.z() - (yaw_drift_slope_ * elapsed_seconds + yaw_drift_offset_); + + compensated_yaw = normalizeAngle(compensated_yaw); + compensated_pitch = normalizeAngle(compensated_pitch); + compensated_roll = normalizeAngle(compensated_roll); + + return {compensated_roll, compensated_pitch, compensated_yaw}; + } + } + + static double normalizeAngle(double angle) { + while (angle > M_PI) angle -= 2 * M_PI; + while (angle < -M_PI) angle += 2 * M_PI; + return angle; + } + + void collectCalibrationSamples(const Eigen::Vector3d& angles, double elapsed_seconds) { + if (sample_counter_ % 100 == 0) { // at 1000Hz，sampling 10 times per sec + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + roll_samples_.push_back(angles.x()); + pitch_samples_.push_back(angles.y()); + yaw_samples_.push_back(angles.z()); + time_samples_.push_back(elapsed_seconds); + } + } + sample_counter_++; + + if (elapsed_seconds >= final_sample_spot_ && !calibration_complete_) { + calculateCompensationParameters(); + calibration_complete_ = true; + RCLCPP_INFO(logger_, "IMU calibration complete with %zu samples", roll_samples_.size()); + clearSamples(); + } + } + + void calculateCompensationParameters() { + if (roll_samples_.empty()) return; + + roll_bias_ = std::accumulate(roll_samples_.begin(), roll_samples_.end(), 0.0) / roll_samples_.size(); + pitch_bias_ = std::accumulate(pitch_samples_.begin(), pitch_samples_.end(), 0.0) / pitch_samples_.size(); + + calculateYawDriftCompensation(); + + RCLCPP_INFO(logger_, "IMU Bias - Roll: %.6f, Pitch: %.6f, Yaw slope: %.6f", + roll_bias_, pitch_bias_, yaw_drift_slope_); + } + + void calculateYawDriftCompensation() { + if (yaw_samples_.size() < 2) return; + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + int n = yaw_samples_.size(); + + for (size_t i = 0; i < n; ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) { + yaw_drift_slope_ = (n * sum_xy - sum_x * sum_y) / denominator; + yaw_drift_offset_ = (sum_y - yaw_drift_slope_ * sum_x) / n; + } + } + + void clearSamples() { + roll_samples_.clear(); + pitch_samples_.clear(); + yaw_samples_.clear(); + time_samples_.clear(); + } + + void setOutputStates(const CompensatedResult& compensated) { + *final_roll = compensated.roll; + *final_pitch = compensated.pitch; + *final_yaw = compensated.yaw; + } + + void setTfTransforms(const CompensatedResult& compensated) { + tf_->set_state(compensated.yaw); + publishTfTransforms(compensated); + } + + void publishTfTransforms(const CompensatedResult& compensated) { + auto now = this->get_clock()->now(); + + auto create_transform = [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; + + auto create_translation = [](double x, double y, double z) { + geometry_msgs::msg::Vector3 t; + t.x = x; t.y = y; t.z = z; + return t; + }; + + auto create_rotation = [](double x, double y, double z, double w) { + geometry_msgs::msg::Quaternion r; + r.x = x; r.y = y; r.z = z; r.w = w; + return r; + }; + + geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); + geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); + + Eigen::Quaterniond yaw_quaternion = + Eigen::AngleAxisd(compensated.yaw, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(compensated.pitch, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(compensated.roll, Eigen::Vector3d::UnitX()); + + // base_link -> gimbal_center_link + tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", + zero_trans, create_rotation(0, 0, 0, 1))); + + // gimbal_center_link -> yaw_link + Eigen::AngleAxisd yaw_axis(compensated.yaw, Eigen::Vector3d::UnitZ()); + Eigen::Quaterniond yaw_only_quaternion(yaw_axis); + tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", + zero_trans, create_rotation(yaw_only_quaternion.x(), yaw_only_quaternion.y(), + yaw_only_quaternion.z(), yaw_only_quaternion.w()))); + + // yaw_link -> pitch_link + Eigen::AngleAxisd pitch_axis(compensated.pitch, Eigen::Vector3d::UnitY()); + Eigen::Quaterniond pitch_only_quaternion(pitch_axis); + tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", + pitch_trans, create_rotation(pitch_only_quaternion.x(), pitch_only_quaternion.y(), + pitch_only_quaternion.z(), pitch_only_quaternion.w()))); + + // pitch_link -> odom_imu + tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", + zero_trans, create_rotation(yaw_quaternion.x(), yaw_quaternion.y(), + yaw_quaternion.z(), yaw_quaternion.w()))); + + // world -> base_link + tf_broadcaster_->sendTransform(create_transform("world", "base_link", + zero_trans, create_rotation(0, 0, 0, 1))); + } + + static Eigen::Vector3d quaternionToEuler(const Eigen::Quaterniond& q) { + Eigen::Matrix3d m = q.toRotationMatrix(); + return {std::atan2(m(2,1), m(2,2)), std::asin(-m(2,0)), std::atan2(m(1,0), m(0,0))}; + } + + double imu_sensitivity_; + double first_sample_spot_; + double final_sample_spot_; + + QuaternionBayesFilter quaternion_filter_{15.0, 1000.0}; + QuaternionBayesFilter quaternion_smoother_{10.0, 1000.0}; + size_t sample_counter_ = 0; + std::vector roll_samples_, pitch_samples_, yaw_samples_, time_samples_; + + double roll_bias_ = 0.0, pitch_bias_ = 0.0, yaw_drift_slope_ = 0.0, yaw_drift_offset_ = 0.0; + + OutputInterface tf_; + std::unique_ptr tf_broadcaster_; + device::Dr16 dr16_; device::Bmi088 imu_; device::DjiMotor pitch_motor_; device::DjiMotor yaw_motor_; device::DjiMotor force_control_motor_; -<<<<<<< HEAD - device::ForceSensor force_sensor_; -======= device::DjiMotor drive_belt_motor_[2]; device::ForceSensorRuntime force_sensor_; device::TriggerServo trigger_servo_; rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; - - double imu_sensitivity_; OutputInterface pitch_angle_; - OutputInterface pitch_velocity_output_; - OutputInterface yaw_velocity_output_; + OutputInterface final_pitch; + OutputInterface final_roll; + OutputInterface final_yaw; }; -<<<<<<< HEAD - -} // namespace rmcs_core::hardware - -#include -======= } // namespace rmcs_core::hardware #include ->>>>>>> b3b6fa5723ff46ff682219460cabef62d2fd622e PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file From 2482524d2d7c0725169cd4285f41694f34ef6e88 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Sat, 3 Jan 2026 21:53:49 +0800 Subject: [PATCH 27/45] an update before pull --- .../rmcs_bringup/config/dart-pitch-pid.yaml | 11 +- .../src/controller/dart/dart_settings.cpp | 9 +- .../src/controller/dart/debug_info.cpp | 19 +- .../rmcs_core/src/hardware/catapult_dart.cpp | 339 +++------- rmcs_ws/src/rmcs_core/src/hardware/test3.cpp | 589 ++++++++++++++++++ rmcs_ws/src/rmcs_core/src/hardware/test5.cpp | 439 +++++++++++++ 6 files changed, 1158 insertions(+), 248 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/test3.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/test5.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml index 6f9833ad..3a3eefde 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml @@ -14,9 +14,8 @@ rmcs_executor: catapult_dart: ros__parameters: usb_pid: -1 - imu_sensitivity: 2.0 - first_sample_spot: 5.0 - final_sample_spot: 10.0 + first_sample_spot: 1.0 + final_sample_spot: 4.0 dart_settings: ros__parameters: @@ -76,6 +75,6 @@ Value_Broadcasters: forward_list: - "/dart/pitch/angle" - "/dart/force_control_motor/velocity" - - "/imu/state/final_yaw" - - "/imu/state/final_pitch" - - "/imu/state/final_roll" \ No newline at end of file + - "/imu/catapult_roll_angle" + - "/imu/catapult_pitch_angle" + - "/imu/catapult_yaw_angle" \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp index c011551f..33ad4e53 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include "controller/pid/pid_calculator.hpp" @@ -96,7 +97,7 @@ class DartSettings *force_control_ = joystick_right_->x() * max_transform_rate_ * 5; break; } else if (is_auto_force_control_mode_ == 1) { - *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) / 2; + *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; transmit_distance_to_force(); force_control(); log_count_++; @@ -135,7 +136,7 @@ class DartSettings } void transmit_distance_to_force() { - force_control_setpoint_ = 3800.0; + force_control_setpoint_ = 3852.0; } private: @@ -143,8 +144,8 @@ class DartSettings rclcpp::Logger logger_; int log_count_ = 0; - int is_auto_pitch_control_mode_; - int is_auto_force_control_mode_; + int64_t is_auto_pitch_control_mode_; + int64_t is_auto_force_control_mode_; double max_transform_rate_; double pitch_angle_setpoint_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp index 756edc3b..2cae5a67 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp @@ -15,17 +15,34 @@ class Test register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); register_input("/dart/pitch_motor/velocity", pitch_speed_); register_input("/dart/yaw_motor/velocity", yaw_speed_); - } + register_input("/imu/catapult_roll_angle", final_roll_); + register_input("/imu/catapult_pitch_angle", final_pitch_); + register_input("/imu/catapult_yaw_angle", final_yaw_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_);} void update() override { + if (count_n == 100) { // RCLCPP_INFO(this->get_logger(), "pitch_control_torque = %f, yaw_control_torque = %f", *pitch_control_torque_, *yaw_control_torque_); // RCLCPP_INFO(this->get_logger(), "pitch_speed = %f, yaw_speed = %f", *pitch_speed_, *yaw_speed_); + RCLCPP_INFO(this->get_logger(), "final_roll = %f, final_pitch = %f, final_yaw = %f", *final_roll_, *final_pitch_, *final_yaw_); + // RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); + count_n = 0; + } + count_n++; } private: + int count_n = 0; rclcpp::Logger logger_; InputInterface pitch_control_torque_; InputInterface yaw_control_torque_; InputInterface pitch_speed_; InputInterface yaw_speed_; + InputInterface final_pitch_; + InputInterface final_roll_; + InputInterface final_yaw_; + InputInterface force_sensor_ch1_data_; + InputInterface force_sensor_ch2_data_; + }; } // namespace rmcs_core::controller::dart diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index ef44e4e8..6d6c7eac 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,55 +1,25 @@ +#include "librmcs/client/cboard.hpp" #include "hardware/device/dji_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/bmi088.hpp" -#include "hardware/device/force_sensor.hpp" -#include "librmcs/client/cboard.hpp" -#include #include "hardware/device/force_sensor_runtime.hpp" #include "hardware/device/trigger_servo.hpp" +#include #include #include #include #include -#include #include "filter/low_pass_filter.hpp" #include #include -#include -#include +#include #include #include +#include #include namespace rmcs_core::hardware { -class QuaternionBayesFilter { -public: - QuaternionBayesFilter(double confidence_ratio, double sample_rate) - : alpha_(confidence_ratio / (confidence_ratio + 1.0)) - , initialized_(false) {} - - Eigen::Quaterniond update(const Eigen::Quaterniond& measurement) { - if (!initialized_) { - belief_ = measurement; - initialized_ = true; - return belief_; - } - - belief_ = belief_.slerp(1.0 - alpha_, measurement); - return belief_; - } - - void reset() { initialized_ = false; } - bool isInitialized() const { return initialized_; } - double getConfidence() const { return alpha_; } - Eigen::Quaterniond getCurrentBelief() const { return belief_; } - -private: - double alpha_; - bool initialized_; - Eigen::Quaterniond belief_; -}; - class CatapultDart : public rmcs_executor::Component , public rclcpp::Node @@ -64,6 +34,9 @@ class CatapultDart , pitch_angle_filter_(20.0, 1000.0) , pitch_velocity_filter_(20.0, 1000.0) , yaw_velocity_filter_(20.0, 1000.0) + , yaw_filter_(5.0, 1000.0) + , pitch_filter_(10.0, 1000.0) + , roll_filter_(10.0, 1000.0) , dart_command_(create_partner_component(get_component_name() + "_command", *this)) , dr16_(*this) , imu_(1000, 0.2, 0.0) @@ -85,24 +58,25 @@ class CatapultDart .set_reversed()}) , force_sensor_(*this) , trigger_servo_(*dart_command_, "/dart/trigger_servo") + //, elevating_motor_left_(*dart_command_, "/dart/elevating_motor_left") + //, elevating_motor_right_(*dart_command_, "/dart/elevating_motor_right") + //, fill_limiting_servo_(*dart_command_, "/dart/fill_limiting_servo") , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { - - register_output("/dart/pitch/angle", pitch_angle_); + register_output("/tf", tf_); - register_output("/imu/state/final_roll", final_roll); - register_output("/imu/state/final_pitch", final_pitch); - register_output("/imu/state/final_yaw", final_yaw); + + register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - imu_sensitivity_ = this->get_parameter("imu_sensitivity").as_double(); first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - + + setup_imu_coordinate_mapping(); imu_sampler_initialize(); tf_broadcaster_ = std::make_unique(*this); start_time_ = std::chrono::steady_clock::now(); - calibration_complete_ = false; - trigger_calibrate_subscription_ = create_subscription( "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { trigger_servo_calibrate_subscription_callback(std::move(msg)); @@ -155,11 +129,6 @@ class CatapultDart trigger_servo_.generate_runtime_command(uart_data_length); const auto trigger_servo_uart_data_ptr = command_buffer.get(); transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); - - // std::string hex_string = bytes_to_hex_string(command_buffer.get(), uart_data_length); - // RCLCPP_INFO( - // this->get_logger(), "UART2(length: %zu): [ %s ]", uart_data_length, - // hex_string.c_str()); } transmit_buffer_.trigger_transmission(); @@ -199,16 +168,10 @@ class CatapultDart } void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - x = static_cast(x / imu_sensitivity_); - y = static_cast(y / imu_sensitivity_); - z = static_cast(z / imu_sensitivity_); imu_.store_accelerometer_status(x, y, z); } void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - x = static_cast(x / imu_sensitivity_); - y = static_cast(y / imu_sensitivity_); - z = static_cast(z / imu_sensitivity_); imu_.store_gyroscope_status(x, y, z); } @@ -217,10 +180,6 @@ class CatapultDart if (!success) { RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); } - - // std::string hex_string = bytes_to_hex_string(data, length); - // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, - // hex_string.c_str()); } void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { @@ -229,11 +188,6 @@ class CatapultDart private: void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { - /* - 标定命令格式： - ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" - 替换data值就行 - */ trigger_servo_.set_calibrate_mode(msg->data); std::unique_ptr command_buffer; @@ -263,11 +217,6 @@ class CatapultDart const auto trigger_servo_uart_data_ptr = command_buffer.get(); transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); - - std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); - RCLCPP_INFO( - this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, - hex_string.c_str()); } static std::string bytes_to_hex_string(const std::byte* data, size_t size) { @@ -289,156 +238,63 @@ class CatapultDart return result; } - rclcpp::Logger logger_; - - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - - bool calibration_complete_ = false; - std::chrono::steady_clock::time_point start_time_; - - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDart& robot) - : dart_(robot) {} - - void update() override { dart_.command_update(); } - - CatapultDart& dart_; - }; - std::shared_ptr dart_command_; + void setup_imu_coordinate_mapping() { + imu_.set_coordinate_mapping( + [](double x, double y, double z) -> std::tuple { + return {x, -y, -z}; + }); + } void imu_sampler_initialize() { start_time_ = std::chrono::steady_clock::now(); - calibration_complete_ = false; - sample_counter_ = 0; - quaternion_filter_.reset(); - quaternion_smoother_.reset(); - clearSamples(); - } - - void processImuData() { - Eigen::Quaterniond raw_quaternion{ - imu_.q0() / imu_sensitivity_, - imu_.q1() / imu_sensitivity_, - imu_.q2() / imu_sensitivity_, - imu_.q3() / imu_sensitivity_ - }; - raw_quaternion.normalize(); - - Eigen::Quaterniond bayes_quaternion = quaternion_filter_.update(raw_quaternion); - - Eigen::Quaterniond smoothed_quaternion = quaternion_smoother_.update(bayes_quaternion); - - Eigen::Vector3d smoothed_euler = quaternionToEuler(smoothed_quaternion); + yaw_drift_coefficient_ = 0.0; - auto compensated = applyDriftCompensation(smoothed_euler); - - setOutputStates(compensated); - - setTfTransforms(compensated); } - struct CompensatedResult { double roll, pitch, yaw; }; - - CompensatedResult applyDriftCompensation(const Eigen::Vector3d& angles) { + void processImuData() { auto current_time = std::chrono::steady_clock::now(); double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); - if (!calibration_complete_) { - collectCalibrationSamples(angles, elapsed_seconds); - return {angles.x(), angles.y(), angles.z()}; - } else { - double compensated_roll = angles.x() - roll_bias_; - double compensated_pitch = angles.y() - pitch_bias_; - double compensated_yaw = angles.z() - (yaw_drift_slope_ * elapsed_seconds + yaw_drift_offset_); - - compensated_yaw = normalizeAngle(compensated_yaw); - compensated_pitch = normalizeAngle(compensated_pitch); - compensated_roll = normalizeAngle(compensated_roll); - - return {compensated_roll, compensated_pitch, compensated_yaw}; - } - } - - static double normalizeAngle(double angle) { - while (angle > M_PI) angle -= 2 * M_PI; - while (angle < -M_PI) angle += 2 * M_PI; - return angle; - } - - void collectCalibrationSamples(const Eigen::Vector3d& angles, double elapsed_seconds) { - if (sample_counter_ % 100 == 0) { // at 1000Hz，sampling 10 times per sec - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - roll_samples_.push_back(angles.x()); - pitch_samples_.push_back(angles.y()); - yaw_samples_.push_back(angles.z()); - time_samples_.push_back(elapsed_seconds); - } - } - sample_counter_++; - - if (elapsed_seconds >= final_sample_spot_ && !calibration_complete_) { - calculateCompensationParameters(); - calibration_complete_ = true; - RCLCPP_INFO(logger_, "IMU calibration complete with %zu samples", roll_samples_.size()); - clearSamples(); - } - } - - void calculateCompensationParameters() { - if (roll_samples_.empty()) return; - - roll_bias_ = std::accumulate(roll_samples_.begin(), roll_samples_.end(), 0.0) / roll_samples_.size(); - pitch_bias_ = std::accumulate(pitch_samples_.begin(), pitch_samples_.end(), 0.0) / pitch_samples_.size(); + Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - calculateYawDriftCompensation(); + Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - RCLCPP_INFO(logger_, "IMU Bias - Roll: %.6f, Pitch: %.6f, Yaw slope: %.6f", - roll_bias_, pitch_bias_, yaw_drift_slope_); - } - - void calculateYawDriftCompensation() { - if (yaw_samples_.size() < 2) return; - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - int n = yaw_samples_.size(); + double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); + double pitch = std::asin(-rotation_matrix(2,0)); + double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); - for (size_t i = 0; i < n; ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } + double transformed_roll = -roll_filter_.update(roll); + double transformed_pitch = pitch_filter_.update(pitch); + double transformed_yaw = -yaw_filter_.update(yaw); - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) { - yaw_drift_slope_ = (n * sum_xy - sum_x * sum_y) / denominator; - yaw_drift_offset_ = (sum_y - yaw_drift_slope_ * sum_x) / n; + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + yaw_samples_.push_back(transformed_yaw); + time_samples_.push_back(elapsed_seconds); + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + double n = yaw_samples_.size(); + + for (int i = 0; i < n; ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) { + yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; + } } - } - - void clearSamples() { - roll_samples_.clear(); - pitch_samples_.clear(); - yaw_samples_.clear(); - time_samples_.clear(); - } - - void setOutputStates(const CompensatedResult& compensated) { - *final_roll = compensated.roll; - *final_pitch = compensated.pitch; - *final_yaw = compensated.yaw; - } + + transformed_yaw -= ((yaw_drift_coefficient_ + 0.000512) * elapsed_seconds); + publishTfTransforms(transformed_roll, transformed_pitch, transformed_yaw); - void setTfTransforms(const CompensatedResult& compensated) { - tf_->set_state(compensated.yaw); - publishTfTransforms(compensated); + *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; + *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; + *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; } - void publishTfTransforms(const CompensatedResult& compensated) { + void publishTfTransforms(double roll, double pitch, double yaw) { auto now = this->get_clock()->now(); auto create_transform = [&](const std::string& parent, const std::string& child, @@ -464,59 +320,61 @@ class CatapultDart r.x = x; r.y = y; r.z = z; r.w = w; return r; }; - geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - Eigen::Quaterniond yaw_quaternion = - Eigen::AngleAxisd(compensated.yaw, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(compensated.pitch, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(compensated.roll, Eigen::Vector3d::UnitX()); + // Create quaternions for each axis rotation + Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + + // Combined rotation: yaw -> pitch -> roll + Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - // base_link -> gimbal_center_link + // Publish all TF transforms tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", zero_trans, create_rotation(0, 0, 0, 1))); - // gimbal_center_link -> yaw_link - Eigen::AngleAxisd yaw_axis(compensated.yaw, Eigen::Vector3d::UnitZ()); - Eigen::Quaterniond yaw_only_quaternion(yaw_axis); tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", - zero_trans, create_rotation(yaw_only_quaternion.x(), yaw_only_quaternion.y(), - yaw_only_quaternion.z(), yaw_only_quaternion.w()))); + zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), + yaw_quat.z(), yaw_quat.w()))); - // yaw_link -> pitch_link - Eigen::AngleAxisd pitch_axis(compensated.pitch, Eigen::Vector3d::UnitY()); - Eigen::Quaterniond pitch_only_quaternion(pitch_axis); tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", - pitch_trans, create_rotation(pitch_only_quaternion.x(), pitch_only_quaternion.y(), - pitch_only_quaternion.z(), pitch_only_quaternion.w()))); + pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), + pitch_quat.z(), pitch_quat.w()))); - // pitch_link -> odom_imu tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", - zero_trans, create_rotation(yaw_quaternion.x(), yaw_quaternion.y(), - yaw_quaternion.z(), yaw_quaternion.w()))); + zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), + combined_quaternion.z(), combined_quaternion.w()))); - // world -> base_link tf_broadcaster_->sendTransform(create_transform("world", "base_link", zero_trans, create_rotation(0, 0, 0, 1))); } - static Eigen::Vector3d quaternionToEuler(const Eigen::Quaterniond& q) { - Eigen::Matrix3d m = q.toRotationMatrix(); - return {std::atan2(m(2,1), m(2,2)), std::asin(-m(2,0)), std::atan2(m(1,0), m(0,0))}; - } + rclcpp::Logger logger_; + + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + filter::LowPassFilter<1> yaw_filter_; + filter::LowPassFilter<1> pitch_filter_; + filter::LowPassFilter<1> roll_filter_; + + std::chrono::steady_clock::time_point start_time_; + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDart& robot) + : dart_(robot) {} - double imu_sensitivity_; + void update() override { dart_.command_update(); } + + CatapultDart& dart_; + }; + std::shared_ptr dart_command_; double first_sample_spot_; double final_sample_spot_; - QuaternionBayesFilter quaternion_filter_{15.0, 1000.0}; - QuaternionBayesFilter quaternion_smoother_{10.0, 1000.0}; - size_t sample_counter_ = 0; - std::vector roll_samples_, pitch_samples_, yaw_samples_, time_samples_; - - double roll_bias_ = 0.0, pitch_bias_ = 0.0, yaw_drift_slope_ = 0.0, yaw_drift_offset_ = 0.0; - OutputInterface tf_; std::unique_ptr tf_broadcaster_; @@ -529,16 +387,23 @@ class CatapultDart device::ForceSensorRuntime force_sensor_; device::TriggerServo trigger_servo_; + // device::TriggerServo elevating_motor_left_; + // device::TriggerServo elevating_motor_right_; + // device::TriggerServo fill_limiting_servo_; rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; - OutputInterface pitch_angle_; - OutputInterface final_pitch; - OutputInterface final_roll; - OutputInterface final_yaw; + OutputInterface catapult_pitch_angle_; + OutputInterface catapult_roll_angle_; + OutputInterface catapult_yaw_angle_; + OutputInterface yaw_samples_output_; + + double yaw_drift_coefficient_ = 0.0; + + std::vector yaw_samples_, time_samples_; }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp new file mode 100644 index 00000000..fdf11e56 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp @@ -0,0 +1,589 @@ +// #include "librmcs/client/cboard.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/force_sensor_runtime.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include +// #include +// #include +// #include +// #include +// #include "filter/low_pass_filter.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware { + +// class CatapultDart +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// CatapultDart() +// : 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()) +// , pitch_angle_filter_(20.0, 1000.0) +// , pitch_velocity_filter_(20.0, 1000.0) +// , yaw_velocity_filter_(20.0, 1000.0) +// , yaw_filter_(5.0, 1000.0) +// , pitch_filter_(10.0, 1000.0) +// , roll_filter_(10.0, 1000.0) +// , dart_command_(create_partner_component(get_component_name() + "_command", *this)) +// , dr16_(*this) +// , imu_(1000, 0.2, 0.0) +// , pitch_motor_( +// *this, *dart_command_, "/dart/pitch_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , yaw_motor_( +// *this, *dart_command_, "/dart/yaw_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , force_control_motor_( +// *this, *dart_command_, "/dart/force_control_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , drive_belt_motor_( +// {*this, *dart_command_, "/dart/drive_belt/left", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, +// {*this, *dart_command_, "/dart/drive_belt/right", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(19.) +// .set_reversed()}) +// , force_sensor_(*this) +// , trigger_servo_(*dart_command_, "/dart/trigger_servo") +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// // Register outputs for TF and IMU state +// register_output("/dart/pitch/angle", pitch_angle_); +// register_output("/tf", tf_); +// register_output("/imu/state/final_roll", final_roll); +// register_output("/imu/state/final_pitch", final_pitch); +// register_output("/imu/state/final_yaw", final_yaw); + +// // Debug outputs for raw angles from IMU (no transformation) +// register_output("/imu/debug/raw_roll", debug_raw_roll_); +// register_output("/imu/debug/raw_pitch", debug_raw_pitch_); +// register_output("/imu/debug/raw_yaw", debug_raw_yaw_); + +// // Debug outputs for transformed angles (unnormalized and normalized) +// register_output("/imu/debug/transformed_roll_unnorm", debug_transformed_roll_unnorm_); +// register_output("/imu/debug/transformed_pitch_unnorm", debug_transformed_pitch_unnorm_); +// register_output("/imu/debug/transformed_yaw_unnorm", debug_transformed_yaw_unnorm_); + +// register_output("/imu/debug/transformed_roll_norm", debug_transformed_roll_norm_); +// register_output("/imu/debug/transformed_pitch_norm", debug_transformed_pitch_norm_); +// register_output("/imu/debug/transformed_yaw_norm", debug_transformed_yaw_norm_); + +// // Additional debug outputs for drift compensation +// register_output("/imu/debug/drift_coefficient", debug_drift_coefficient_); +// register_output("/imu/debug/calibration_complete", debug_calibration_complete_); +// register_output("/imu/debug/compensation_applied", debug_compensation_applied_); + +// // Get IMU parameters +// imu_sensitivity_ = this->get_parameter("imu_sensitivity").as_double(); +// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); +// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + +// // Get initial offset parameters (default to 0.0) +// this->declare_parameter("roll_offset", 0.0); +// this->declare_parameter("pitch_offset", 0.0); +// this->declare_parameter("yaw_offset", 0.0); + +// roll_offset_ = this->get_parameter("roll_offset").as_double(); +// pitch_offset_ = this->get_parameter("pitch_offset").as_double(); +// yaw_offset_ = this->get_parameter("yaw_offset").as_double(); + +// setup_imu_coordinate_mapping(); +// imu_sampler_initialize(); +// tf_broadcaster_ = std::make_unique(*this); +// start_time_ = std::chrono::steady_clock::now(); + +// // Create subscription for trigger calibration +// trigger_calibrate_subscription_ = create_subscription( +// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback(std::move(msg)); +// }); +// } + +// ~CatapultDart() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// dr16_.update_status(); + +// pitch_motor_.update_status(); +// yaw_motor_.update_status(); +// drive_belt_motor_[0].update_status(); +// drive_belt_motor_[1].update_status(); +// force_control_motor_.update_status(); +// force_sensor_.update_status(); + +// imu_.update_status(); +// processImuData(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// if (pub_time_count_++ > 100) { +// transmit_buffer_.add_can1_transmission( +// 0x301, std::bit_cast(force_sensor_.generate_command())); +// pub_time_count_ = 0; +// } + +// can_commands[0] = pitch_motor_.generate_command(); +// can_commands[1] = yaw_motor_.generate_command(); +// can_commands[2] = force_control_motor_.generate_command(); +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// can_commands[0] = drive_belt_motor_[0].generate_command(); +// can_commands[1] = drive_belt_motor_[1].generate_command(); +// can_commands[2] = 0; +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + +// if (!trigger_servo_.calibrate_mode()) { +// size_t uart_data_length; +// std::unique_ptr command_buffer = +// trigger_servo_.generate_runtime_command(uart_data_length); +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); +// } + +// transmit_buffer_.trigger_transmission(); +// } +// int pub_time_count_ = 0; + +// 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 == 0x302) { +// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x201) { +// pitch_motor_.store_status(can_data); +// } else if (can_id == 0x202) { +// yaw_motor_.store_status(can_data); +// } else if (can_id == 0x203) { +// force_control_motor_.store_status(can_data); +// } else if (can_id == 0x205) { +// drive_belt_motor_[0].store_status(can_data); +// } else if (can_id == 0x206) { +// drive_belt_motor_[1].store_status(can_data); +// } +// } + +// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); +// if (!success) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// } + +// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { +// dr16_.store_status(uart_data, uart_data_length); +// } + +// private: +// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { +// trigger_servo_.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; +// if (msg->data == 0) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) { +// return "[]"; +// } + +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); + +// for (size_t i = 0; i < size; ++i) { +// ss << std::setw(2) << static_cast(data[i]) << " "; +// } + +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') { +// result.pop_back(); +// } +// return result; +// } + +// void setup_imu_coordinate_mapping() { +// // Setup coordinate mapping for IMU raw data +// imu_.set_coordinate_mapping( +// [](double x, double y, double z) -> std::tuple { +// return {x, -y, -z}; +// }); +// } + +// void imu_sampler_initialize() { +// // Initialize IMU sampler variables +// start_time_ = std::chrono::steady_clock::now(); +// calibration_started_ = false; +// calibration_complete_ = false; +// yaw_drift_coefficient_ = 0.0; +// yaw_at_first_sample_ = 0.0; +// yaw_at_final_sample_ = 0.0; + +// RCLCPP_INFO(logger_, "IMU sampler initialized"); +// } + +// void processImuData() { +// auto current_time = std::chrono::steady_clock::now(); +// double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); + +// if (!calibration_started_) { +// calibration_started_ = true; +// RCLCPP_INFO(logger_, "IMU calibration started at time: %f seconds", elapsed_seconds); +// } + +// // Get quaternion from IMU +// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + +// // Convert quaternion to rotation matrix +// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); + +// // Extract Euler angles (roll, pitch, yaw) from rotation matrix +// // Using convention: roll (X), pitch (Y), yaw (Z) +// double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); +// double pitch = std::asin(-rotation_matrix(2,0)); +// double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); + +// // Output raw angles without any transformation +// *debug_raw_roll_ = roll; +// *debug_raw_pitch_ = pitch; +// *debug_raw_yaw_ = yaw; + +// // Apply transformations for inverted IMU mounting: +// // 1. 给俯仰角（pitch）补偿π +// // 2. 俯仰和偏航的增量取反（乘以-1） +// // 3. Roll轴保持不变（根据您的描述，roll可能不需要特殊处理） +// double transformed_roll = -roll ; +// double transformed_pitch = pitch ; // 增量取反并补偿π +// double transformed_yaw = -yaw; // 增量取反 + +// // Apply initial offsets (if any) +// transformed_roll -= roll_offset_; +// transformed_pitch -= pitch_offset_; +// transformed_yaw -= yaw_offset_; + +// // Output unnormalized transformed angles +// *debug_transformed_roll_unnorm_ = (transformed_roll / M_PI) * 180; +// *debug_transformed_pitch_unnorm_ = (transformed_pitch / M_PI) * 180; +// *debug_transformed_yaw_unnorm_ = (transformed_yaw /M_PI) * 180; + +// // Normalize angles to [-π, π] +// transformed_roll = normalizeAngle(transformed_roll); +// transformed_pitch = normalizeAngle(transformed_pitch); +// transformed_yaw = normalizeAngle(transformed_yaw); + +// // Output normalized transformed angles +// *debug_transformed_roll_norm_ = transformed_roll; +// *debug_transformed_pitch_norm_ = transformed_pitch; +// *debug_transformed_yaw_norm_ = transformed_yaw; + +// // Apply low-pass filtering to normalized angles +// double filtered_roll = roll_filter_.update(transformed_roll); +// double filtered_pitch = pitch_filter_.update(transformed_pitch); +// double filtered_yaw = yaw_filter_.update(transformed_yaw); + +// // Yaw drift calibration using two-point method (use filtered yaw) +// if (!calibration_complete_) { +// // First sample point +// if (elapsed_seconds >= first_sample_spot_ && +// std::abs(yaw_at_first_sample_) < 0.001) { // Use tolerance for comparison + +// yaw_at_first_sample_ = filtered_yaw; +// first_sample_time_ = elapsed_seconds; +// RCLCPP_INFO(logger_, "First yaw sample: %f rad at time: %f s", +// yaw_at_first_sample_, first_sample_time_); +// } + +// // Final sample point +// if (elapsed_seconds >= final_sample_spot_ && +// std::abs(yaw_at_final_sample_) < 0.001) { + +// yaw_at_final_sample_ = filtered_yaw; +// final_sample_time_ = elapsed_seconds; + +// // Calculate drift coefficient (radians per second) +// if (std::abs(yaw_at_first_sample_) > 0.001) { // Ensure first sample is recorded +// double time_diff = final_sample_time_ - first_sample_time_; +// double yaw_diff = yaw_at_final_sample_ - yaw_at_first_sample_; + +// if (time_diff > 0.1) { // Ensure sufficient time difference +// yaw_drift_coefficient_ = yaw_diff / time_diff; +// calibration_complete_ = true; + +// RCLCPP_INFO(logger_, "Yaw calibration complete"); +// RCLCPP_INFO(logger_, " First sample: %f rad at %f s", +// yaw_at_first_sample_, first_sample_time_); +// RCLCPP_INFO(logger_, " Final sample: %f rad at %f s", +// yaw_at_final_sample_, final_sample_time_); +// RCLCPP_INFO(logger_, " Time difference: %f s", time_diff); +// RCLCPP_INFO(logger_, " Yaw difference: %f rad", yaw_diff); +// RCLCPP_INFO(logger_, " Drift coefficient: %f rad/s", +// yaw_drift_coefficient_); +// } +// } +// } +// } + +// // Apply yaw drift compensation (use filtered yaw) +// double compensated_yaw = filtered_yaw; +// bool compensation_applied = false; + +// if (calibration_complete_) { +// // Calculate time since calibration start +// double time_since_calibration = elapsed_seconds - first_sample_time_; + +// // Apply linear drift compensation to filtered yaw +// compensated_yaw = filtered_yaw - (yaw_drift_coefficient_ * time_since_calibration); +// compensation_applied = true; + +// // Normalize compensated yaw +// compensated_yaw = normalizeAngle(compensated_yaw); +// } + +// // Output final normalized angles (use filtered roll/pitch and compensated yaw) +// *final_roll = filtered_roll; +// *final_pitch = filtered_pitch; +// *final_yaw = compensated_yaw; + +// // Output debug information +// *debug_drift_coefficient_ = yaw_drift_coefficient_; +// *debug_calibration_complete_ = calibration_complete_ ? 1.0 : 0.0; +// *debug_compensation_applied_ = compensation_applied ? 1.0 : 0.0; + +// // Publish TF transforms using filtered roll/pitch and compensated yaw +// // 注意：需要将转换应用到odom坐标系 +// publishTfTransforms(filtered_roll, filtered_pitch, compensated_yaw); +// } + +// static double normalizeAngle(double angle) { +// // Normalize angle to range [-π, π] +// while (angle > M_PI) angle -= 2.0 * M_PI; +// while (angle < -M_PI) angle += 2.0 * M_PI; +// return angle; +// } + +// void publishTfTransforms(double roll, double pitch, double yaw) { +// auto now = this->get_clock()->now(); + +// // Helper function to create transform +// auto create_transform = [&](const std::string& parent, const std::string& child, +// const geometry_msgs::msg::Vector3& trans, +// const geometry_msgs::msg::Quaternion& rot) { +// geometry_msgs::msg::TransformStamped tf; +// tf.header.stamp = now; +// tf.header.frame_id = parent; +// tf.child_frame_id = child; +// tf.transform.translation = trans; +// tf.transform.rotation = rot; +// return tf; +// }; + +// // Helper function to create translation +// auto create_translation = [](double x, double y, double z) { +// geometry_msgs::msg::Vector3 t; +// t.x = x; t.y = y; t.z = z; +// return t; +// }; + +// // Helper function to create rotation +// auto create_rotation = [](double x, double y, double z, double w) { +// geometry_msgs::msg::Quaternion r; +// r.x = x; r.y = y; r.z = z; r.w = w; +// return r; +// }; + +// // Create zero translation +// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); +// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); + +// // Create quaternions for each axis rotation +// // 注意：这里使用的roll, pitch, yaw已经是经过转换、滤波和补偿的最终值 +// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); +// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); +// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + +// // Combined rotation: yaw -> pitch -> roll +// // 这个组合顺序需要根据您的TF树结构来确定 +// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; + +// // Publish all TF transforms +// // 注意：将最终转换应用到odom_imu坐标系 +// tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", +// zero_trans, create_rotation(0, 0, 0, 1))); + +// // Yaw only rotation +// tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", +// zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), +// yaw_quat.z(), yaw_quat.w()))); + +// // Pitch only rotation (注意：这里使用了转换后的pitch值) +// tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", +// pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), +// pitch_quat.z(), pitch_quat.w()))); + +// // Combined rotation for odom_imu frame (应用了所有转换) +// tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", +// zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), +// combined_quaternion.z(), combined_quaternion.w()))); + +// // World to base_link transform +// tf_broadcaster_->sendTransform(create_transform("world", "base_link", +// zero_trans, create_rotation(0, 0, 0, 1))); +// } + +// rclcpp::Logger logger_; + +// // Low-pass filters for IMU data +// filter::LowPassFilter<1> pitch_angle_filter_; +// filter::LowPassFilter<1> pitch_velocity_filter_; +// filter::LowPassFilter<1> yaw_velocity_filter_; +// filter::LowPassFilter<1> yaw_filter_; +// filter::LowPassFilter<1> pitch_filter_; +// filter::LowPassFilter<1> roll_filter_; + +// std::chrono::steady_clock::time_point start_time_; + +// // Dart command component +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDart& robot) +// : dart_(robot) {} + +// void update() override { dart_.command_update(); } + +// CatapultDart& dart_; +// }; +// std::shared_ptr dart_command_; + +// // IMU parameters +// double imu_sensitivity_; +// double first_sample_spot_; +// double final_sample_spot_; +// double roll_offset_; +// double pitch_offset_; +// double yaw_offset_; + +// // TF related +// OutputInterface tf_; +// std::unique_ptr tf_broadcaster_; + +// // Hardware devices +// device::Dr16 dr16_; +// device::Bmi088 imu_; +// device::DjiMotor pitch_motor_; +// device::DjiMotor yaw_motor_; +// device::DjiMotor force_control_motor_; +// device::DjiMotor drive_belt_motor_[2]; + +// device::ForceSensorRuntime force_sensor_; +// device::TriggerServo trigger_servo_; + +// // Subscriptions +// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; + +// // Output interfaces for IMU data +// OutputInterface pitch_angle_; +// OutputInterface final_roll; +// OutputInterface final_pitch; +// OutputInterface final_yaw; + +// // Debug output interfaces for raw IMU data +// OutputInterface debug_raw_roll_; +// OutputInterface debug_raw_pitch_; +// OutputInterface debug_raw_yaw_; + +// // Debug output interfaces (only transformed angles) +// OutputInterface debug_transformed_roll_unnorm_; +// OutputInterface debug_transformed_pitch_unnorm_; +// OutputInterface debug_transformed_yaw_unnorm_; +// OutputInterface debug_transformed_roll_norm_; +// OutputInterface debug_transformed_pitch_norm_; +// OutputInterface debug_transformed_yaw_norm_; + +// // Debug output for drift compensation +// OutputInterface debug_drift_coefficient_; +// OutputInterface debug_calibration_complete_; +// OutputInterface debug_compensation_applied_; + +// // IMU calibration variables +// bool calibration_started_ = false; +// bool calibration_complete_ = false; +// double yaw_drift_coefficient_ = 0.0; +// double yaw_at_first_sample_ = 0.0; +// double yaw_at_final_sample_ = 0.0; +// double first_sample_time_ = 0.0; +// double final_sample_time_ = 0.0; +// }; +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp b/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp new file mode 100644 index 00000000..fb0f7af7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp @@ -0,0 +1,439 @@ +// #include "librmcs/client/cboard.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/force_sensor_runtime.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include +// #include +// #include +// #include +// #include +// #include "filter/low_pass_filter.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware { + +// class CatapultDart +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// CatapultDart() +// : 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()) +// , pitch_angle_filter_(20.0, 1000.0) +// , pitch_velocity_filter_(20.0, 1000.0) +// , yaw_velocity_filter_(20.0, 1000.0) +// , yaw_filter_(5.0, 1000.0) +// , pitch_filter_(10.0, 1000.0) +// , roll_filter_(10.0, 1000.0) +// , dart_command_(create_partner_component(get_component_name() + "_command", *this)) +// , dr16_(*this) +// , imu_(1000, 0.2, 0.0) +// , pitch_motor_( +// *this, *dart_command_, "/dart/pitch_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , yaw_motor_( +// *this, *dart_command_, "/dart/yaw_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , force_control_motor_( +// *this, *dart_command_, "/dart/force_control_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , drive_belt_motor_( +// {*this, *dart_command_, "/dart/drive_belt/left", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, +// {*this, *dart_command_, "/dart/drive_belt/right", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(19.) +// .set_reversed()}) +// , force_sensor_(*this) +// , trigger_servo_(*dart_command_, "/dart/trigger_servo") +// //, elevating_motor_left_(*dart_command_, "/dart/elevating_motor_left") +// //, elevating_motor_right_(*dart_command_, "/dart/elevating_motor_right") +// //, fill_limiting_servo_(*dart_command_, "/dart/fill_limiting_servo") +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// register_output("/tf", tf_); + +// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); +// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); +// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + +// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); +// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + +// setup_imu_coordinate_mapping(); +// imu_sampler_initialize(); +// tf_broadcaster_ = std::make_unique(*this); +// start_time_ = std::chrono::steady_clock::now(); +// trigger_calibrate_subscription_ = create_subscription( +// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback(std::move(msg)); +// }); +// } + +// ~CatapultDart() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// dr16_.update_status(); + +// pitch_motor_.update_status(); +// yaw_motor_.update_status(); +// drive_belt_motor_[0].update_status(); +// drive_belt_motor_[1].update_status(); +// force_control_motor_.update_status(); +// force_sensor_.update_status(); + +// imu_.update_status(); +// processImuData(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// if (pub_time_count_++ > 100) { +// transmit_buffer_.add_can1_transmission( +// 0x301, std::bit_cast(force_sensor_.generate_command())); +// pub_time_count_ = 0; +// } + +// can_commands[0] = pitch_motor_.generate_command(); +// can_commands[1] = yaw_motor_.generate_command(); +// can_commands[2] = force_control_motor_.generate_command(); +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// can_commands[0] = drive_belt_motor_[0].generate_command(); +// can_commands[1] = drive_belt_motor_[1].generate_command(); +// can_commands[2] = 0; +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + +// if (!trigger_servo_.calibrate_mode()) { +// size_t uart_data_length; +// std::unique_ptr command_buffer = +// trigger_servo_.generate_runtime_command(uart_data_length); +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); +// } + +// transmit_buffer_.trigger_transmission(); +// } +// int pub_time_count_ = 0; + +// 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 == 0x302) { +// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x201) { +// pitch_motor_.store_status(can_data); +// } else if (can_id == 0x202) { +// yaw_motor_.store_status(can_data); +// } else if (can_id == 0x203) { +// force_control_motor_.store_status(can_data); +// } else if (can_id == 0x205) { +// drive_belt_motor_[0].store_status(can_data); +// } else if (can_id == 0x206) { +// drive_belt_motor_[1].store_status(can_data); +// } +// } + +// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); +// if (!success) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// } + +// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { +// dr16_.store_status(uart_data, uart_data_length); +// } + +// private: +// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { +// trigger_servo_.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; +// if (msg->data == 0) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) { +// return "[]"; +// } + +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); + +// for (size_t i = 0; i < size; ++i) { +// ss << std::setw(2) << static_cast(data[i]) << " "; +// } + +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') { +// result.pop_back(); +// } +// return result; +// } + +// void setup_imu_coordinate_mapping() { +// imu_.set_coordinate_mapping( +// [](double x, double y, double z) -> std::tuple { +// return {x, -y, -z}; +// }); +// } + +// void imu_sampler_initialize() { +// start_time_ = std::chrono::steady_clock::now(); +// calibration_started_ = false; +// calibration_complete_ = false; +// yaw_drift_coefficient_ = 0.0; +// first_sample_taken_ = false; +// final_sample_taken_ = false; + +// } + +// void processImuData() { +// auto current_time = std::chrono::steady_clock::now(); +// double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); + +// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + +// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); + +// double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); +// double pitch = std::asin(-rotation_matrix(2,0)); +// double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); + +// double transformed_roll = -roll_filter_.update(roll); +// double transformed_pitch = pitch_filter_.update(pitch); +// double transformed_yaw = -yaw_filter_.update(yaw); + +// *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; +// *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; +// *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; + +// transformed_roll = normalizeAngle(transformed_roll); +// transformed_pitch = normalizeAngle(transformed_pitch); +// transformed_yaw = normalizeAngle(transformed_yaw); + +// double filtered_roll = roll_filter_.update(transformed_roll); +// double filtered_pitch = pitch_filter_.update(transformed_pitch); +// double filtered_yaw = yaw_filter_.update(transformed_yaw); + +// if (!calibration_complete_) { +// if (elapsed_seconds >= final_sample_spot_ && !final_sample_taken_) { +// double yaw_at_first_sample = filtered_yaw; +// double yaw_at_final_sample = filtered_yaw; +// double final_sample_time = elapsed_seconds; +// final_sample_taken_ = true; + +// double time_diff = final_sample_time - first_sample_spot_; +// double yaw_diff = yaw_at_final_sample - yaw_at_first_sample; + +// if (time_diff > 0.1) { +// yaw_drift_coefficient_ = yaw_diff / time_diff; +// calibration_complete_ = true; +// } +// } +// } + +// double compensated_yaw = filtered_yaw; + +// if (calibration_complete_) { +// double time_since_calibration = elapsed_seconds - first_sample_spot_; + +// compensated_yaw = filtered_yaw - (yaw_drift_coefficient_ * time_since_calibration); + +// compensated_yaw = normalizeAngle(compensated_yaw); +// } + +// publishTfTransforms(filtered_roll, filtered_pitch, compensated_yaw); +// } + +// static double normalizeAngle(double angle) { +// while (angle > M_PI) angle -= 2.0 * M_PI; +// while (angle < -M_PI) angle += 2.0 * M_PI; +// return angle; +// } + +// void publishTfTransforms(double roll, double pitch, double yaw) { +// auto now = this->get_clock()->now(); + +// auto create_transform = [&](const std::string& parent, const std::string& child, +// const geometry_msgs::msg::Vector3& trans, +// const geometry_msgs::msg::Quaternion& rot) { +// geometry_msgs::msg::TransformStamped tf; +// tf.header.stamp = now; +// tf.header.frame_id = parent; +// tf.child_frame_id = child; +// tf.transform.translation = trans; +// tf.transform.rotation = rot; +// return tf; +// }; + +// auto create_translation = [](double x, double y, double z) { +// geometry_msgs::msg::Vector3 t; +// t.x = x; t.y = y; t.z = z; +// return t; +// }; + +// auto create_rotation = [](double x, double y, double z, double w) { +// geometry_msgs::msg::Quaternion r; +// r.x = x; r.y = y; r.z = z; r.w = w; +// return r; +// }; +// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); +// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); + +// // Create quaternions for each axis rotation +// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); +// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); +// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + +// // Combined rotation: yaw -> pitch -> roll +// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; + +// // Publish all TF transforms +// tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", +// zero_trans, create_rotation(0, 0, 0, 1))); + +// tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", +// zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), +// yaw_quat.z(), yaw_quat.w()))); + +// tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", +// pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), +// pitch_quat.z(), pitch_quat.w()))); + +// tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", +// zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), +// combined_quaternion.z(), combined_quaternion.w()))); + +// tf_broadcaster_->sendTransform(create_transform("world", "base_link", +// zero_trans, create_rotation(0, 0, 0, 1))); +// } + +// rclcpp::Logger logger_; + +// filter::LowPassFilter<1> pitch_angle_filter_; +// filter::LowPassFilter<1> pitch_velocity_filter_; +// filter::LowPassFilter<1> yaw_velocity_filter_; +// filter::LowPassFilter<1> yaw_filter_; +// filter::LowPassFilter<1> pitch_filter_; +// filter::LowPassFilter<1> roll_filter_; + +// std::chrono::steady_clock::time_point start_time_; + +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDart& robot) +// : dart_(robot) {} + +// void update() override { dart_.command_update(); } + +// CatapultDart& dart_; +// }; +// std::shared_ptr dart_command_; +// double first_sample_spot_; +// double final_sample_spot_; + +// OutputInterface tf_; +// std::unique_ptr tf_broadcaster_; + +// device::Dr16 dr16_; +// device::Bmi088 imu_; +// device::DjiMotor pitch_motor_; +// device::DjiMotor yaw_motor_; +// device::DjiMotor force_control_motor_; +// device::DjiMotor drive_belt_motor_[2]; + +// device::ForceSensorRuntime force_sensor_; +// device::TriggerServo trigger_servo_; +// // device::TriggerServo elevating_motor_left_; +// // device::TriggerServo elevating_motor_right_; +// // device::TriggerServo fill_limiting_servo_; + +// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; + +// OutputInterface catapult_pitch_angle_; +// OutputInterface catapult_roll_angle_; +// OutputInterface catapult_yaw_angle_; + +// bool calibration_started_ = false; +// bool calibration_complete_ = false; +// bool first_sample_taken_ = false; +// bool final_sample_taken_ = false; +// double yaw_drift_coefficient_ = 0.0; +// }; +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file From 3193ad34a363660cbb882e66ec0132374c66e7bb Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Sat, 10 Jan 2026 20:54:58 +0800 Subject: [PATCH 28/45] next step i'll do some encapsulation --- .../rmcs_bringup/config/dart-pitch-pid.yaml | 12 ++++ .../src/controller/dart/launch_controller.cpp | 63 +++++++++++++++++-- .../rmcs_core/src/hardware/catapult_dart.cpp | 15 +++-- 3 files changed, 79 insertions(+), 11 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml index 3a3eefde..58021bef 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml @@ -32,13 +32,25 @@ dart_settings: launch_controller: ros__parameters: belt_velocity: 10.0 + lifting_velocity: 10.0 sync_coefficient: 0.5 + lifting_sync_coefficient: 0.5 b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 + lt_kp: 1.0 + lt_ki: 0.0 + lt_kd: 0.0 max_control_torque: 2.5 trigger_free_angle: 0x0E2A trigger_lock_angle: 0x0FC1 + lifting_up_angle: 0x0000 + lifting_middle_angle: 0x0000 + lifting_down_angle: 0x0000 + limiting_wait_time: 0x0000 + limiting_free_angle_: 0x0000 + limiting_lock_angle_: 0x0000 + actual_speed_and_control_torque_test_node: ros__parameters: {} diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 5aa8b28f..7b8504a0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -1,5 +1,6 @@ #include "controller/pid/matrix_pid_calculator.hpp" #include +#include #include #include #include @@ -38,14 +39,28 @@ class DartLaunchController , logger_(get_logger()) , drive_belt_pid_calculator_( get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()) { + get_parameter("b_kd").as_double()) + , lifting_table_pid_calculator_( + get_parameter("lt_kp").as_double(), get_parameter("lt_ki").as_double(), + get_parameter("lt_kd").as_double()) { dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); max_control_torque_ = get_parameter("max_control_torque").as_double(); + lifting_table_working_velocity_ = get_parameter("lifting_velocity").as_double(); + lifting_sync_coefficient_ = get_parameter("lifting_sync_coefficient").as_double(); + launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); - launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); + launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); + + lifting_up_angle_ = get_parameter("lifting_up_angle").as_int(); + lifting_middle_angle_ = get_parameter("lifting_middle_angle").as_int(); + lifting_down_angle_ = get_parameter("lifting_down_angle").as_int(); + + limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); + limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); + limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); @@ -56,10 +71,12 @@ class DartLaunchController register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); - register_output( - "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); register_output("/dart/trigger_servo/control_angle", trigger_control_angle); + register_output("/dart/lifting_servo/control_angle", lifting_contorl_angle); + register_output("/dart/limiting_servo/control_angle", limiting_contorl_angle); + } void update() override { @@ -79,6 +96,7 @@ class DartLaunchController if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { if (last_launch_stage_ == DartLaunchStages::INIT) { *launch_stage_ = DartLaunchStages::LOADING; + lifting_table_position_flag_ = 1; } else if (last_launch_stage_ == DartLaunchStages::READY) { *launch_stage_ = DartLaunchStages::CANCEL; } @@ -86,6 +104,7 @@ class DartLaunchController if (last_launch_stage_ == DartLaunchStages::READY) { *launch_stage_ = DartLaunchStages::INIT; trigger_lock_flag_ = false; + loading_process(); } else { RCLCPP_INFO(logger_, "Dart has't been loaded !"); } @@ -94,11 +113,13 @@ class DartLaunchController if (blocking_detection()) { if (last_launch_stage_ == DartLaunchStages::LOADING) { *launch_stage_ = DartLaunchStages::RESETTING; + lifting_table_position_flag_ = 2; trigger_lock_flag_ = true; dirve_belt_block_count_ = 0; } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { *launch_stage_ = DartLaunchStages::RESETTING; + lifting_table_position_flag_ = 1; trigger_lock_flag_ = false; dirve_belt_block_count_ = 0; @@ -124,6 +145,16 @@ class DartLaunchController } *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; + *limiting_contorl_angle = limiting_lock_flag_ ? limiting_lock_angle_ : limiting_trigger_angle_; + switch(lifting_table_position_flag_){ + case 0: *lifting_contorl_angle = lifting_up_angle_; + break; + case 1: *lifting_contorl_angle = lifting_middle_angle_; + break; + case 2: *lifting_contorl_angle = lifting_down_angle_; + break; + default: break; + } last_switch_left_ = *switch_left_; last_switch_right_ = *switch_right_; @@ -174,9 +205,15 @@ class DartLaunchController return dirve_belt_block_count_ > 1000 ? true : false; } + void loading_process() { + lifting_table_position_flag_ = 0; + } + int dirve_belt_block_count_ = 0; double dirve_belt_working_velocity_; + double lifting_table_working_velocity_; + rclcpp::Logger logger_; InputInterface joystick_right_; @@ -201,8 +238,24 @@ class DartLaunchController bool trigger_lock_flag_ = false; OutputInterface trigger_control_angle; + uint16_t lifting_up_angle_; + uint16_t lifting_middle_angle_; + uint16_t lifting_down_angle_; + int lifting_table_position_flag_ = 0; //0: up, 1: middle, 2:down + OutputInterface lifting_contorl_angle; + + uint16_t limiting_lock_angle_; + uint16_t limiting_trigger_angle_; + uint16_t limiting_wait_time_; + bool limiting_lock_flag_ = false; + + OutputInterface limiting_contorl_angle; + pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; double sync_coefficient_; + + pid::MatrixPidCalculator<2> lifting_table_pid_calculator_; + double lifting_sync_coefficient_; }; } // namespace rmcs_core::controller::dart diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 6d6c7eac..99e84632 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -6,6 +6,7 @@ #include "hardware/device/trigger_servo.hpp" #include #include +#include #include #include #include @@ -58,9 +59,9 @@ class CatapultDart .set_reversed()}) , force_sensor_(*this) , trigger_servo_(*dart_command_, "/dart/trigger_servo") - //, elevating_motor_left_(*dart_command_, "/dart/elevating_motor_left") - //, elevating_motor_right_(*dart_command_, "/dart/elevating_motor_right") - //, fill_limiting_servo_(*dart_command_, "/dart/fill_limiting_servo") + , elevating_left_(*dart_command_, "/dart/lifting_left") + , elevating_right_(*dart_command_, "/dart/lifting_right") + , limiting_servo_(*dart_command_, "/dart/limiting_servo") , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { @@ -69,6 +70,7 @@ class CatapultDart register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); register_output("/imu/catapult_roll_angle", catapult_roll_angle_); register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + register_output("/elevating/angle", elevating_angle_); first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); @@ -387,9 +389,9 @@ class CatapultDart device::ForceSensorRuntime force_sensor_; device::TriggerServo trigger_servo_; - // device::TriggerServo elevating_motor_left_; - // device::TriggerServo elevating_motor_right_; - // device::TriggerServo fill_limiting_servo_; + device::TriggerServo elevating_left_; + device::TriggerServo elevating_right_; + device::TriggerServo limiting_servo_; rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; @@ -400,6 +402,7 @@ class CatapultDart OutputInterface catapult_roll_angle_; OutputInterface catapult_yaw_angle_; OutputInterface yaw_samples_output_; + OutputInterface elevating_angle_; double yaw_drift_coefficient_ = 0.0; From 9d6b11b43c39b2277e58481c49620c6ce8067ac6 Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Wed, 25 Feb 2026 22:38:04 +0800 Subject: [PATCH 29/45] developing filling system :refactor servo protocol --- .../config/dart-filling-test.yaml | 31 + .../rmcs_bringup/config/dart-guidance.yaml | 61 ++ .../rmcs_bringup/config/dart-pitch-pid.yaml | 28 +- rmcs_ws/src/rmcs_bringup/config/dart.yaml | 90 ++- rmcs_ws/src/rmcs_core/plugins.xml | 8 +- .../src/controller/dart/dart_filling_test.cpp | 192 ++++++ .../src/controller/dart/dart_settings.cpp | 10 +- .../src/controller/dart/debug_info.cpp | 247 ++++++- .../src/controller/dart/launch_controller.cpp | 180 +++-- .../src/controller/dart/servo_test.cpp | 35 +- .../rmcs_core/src/hardware/catapult_dart.cpp | 283 +++++--- .../hardware/dart_filling_test_hardware.cpp | 316 +++++++++ .../src/hardware/device/hojo_board.hpp | 96 +++ .../src/hardware/device/lifting_left.hpp | 175 +++++ .../src/hardware/device/lifting_right.hpp | 176 +++++ .../src/hardware/device/limiting_servo.hpp | 622 ++++++++++++++++++ .../src/hardware/device/trigger_servo.hpp | 365 ++++++++-- rmcs_ws/src/rmcs_core/src/hardware/test3.cpp | 589 ----------------- rmcs_ws/src/rmcs_core/src/hardware/test5.cpp | 439 ------------ rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt | 97 +++ rmcs_ws/src/rmcs_dart_guidance/package.xml | 32 + rmcs_ws/src/rmcs_dart_guidance/plugins.xml | 21 + .../src/vision/angle_solver.hpp | 142 ++++ .../src/vision/dart_vision_core.cpp | 341 ++++++++++ .../src/vision/identifier.hpp | 191 ++++++ .../rmcs_dart_guidance/src/vision/tracker.hpp | 112 ++++ .../test/debug_image_display.cpp | 321 +++++++++ .../test/get_camera_parameter.cpp | 498 ++++++++++++++ .../test/image_publisher.cpp | 58 ++ .../test/image_saving_test.cpp | 118 ++++ .../include/rmcs_msgs/dart_launch_stage.hpp | 17 + 31 files changed, 4598 insertions(+), 1293 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/test3.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/test5.cpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt create mode 100644 rmcs_ws/src/rmcs_dart_guidance/package.xml create mode 100644 rmcs_ws/src/rmcs_dart_guidance/plugins.xml create mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp create mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml new file mode 100644 index 00000000..629c850f --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml @@ -0,0 +1,31 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::DartFillingTestHardware -> catapult_dart + - rmcs_core::controller::dart::DartFillingTest -> launch_controller + # - rmcs_core::controller::dart::Test -> actual_speed_and_control_torque_test_node + +catapult_dart: + ros__parameters: + usb_pid: -1 + +launch_controller: + ros__parameters: + lifting_up_angle_left: 0x0656 + lifting_down_angle_left: 0x0DE3 + lifting_up_angle_right: 0x08C7 + lifting_down_angle_right: 0x0C54 + trigger_free_angle: 0x0E2A + trigger_lock_angle: 0x0FC1 + limiting_wait_time: 0x0010 + limiting_free_angle_: 0x0001 + limiting_lock_angle_: 0x0050 + + +# actual_speed_and_control_torque_test_node: +# ros__parameters: +# lifting_up_angle_left: 0x0656 +# lifting_down_angle_left: 0x0DE3 +# lifting_up_angle_right: 0x08C7 +# lifting_down_angle_right: 0x0C54 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml new file mode 100644 index 00000000..4f3cd01a --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -0,0 +1,61 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_dart_guidance::DartVisionCore -> dart_vision_core + - rmcs_dart_guidance::DebugDisplayComponent -> debug_display + - rmcs_dart_guidance::ImagePublisher -> image_publisher + - rmcs_dart_guidance::ImagePublisher -> camera_parameter_image_publisher + - rmcs_dart_guidance::CameraCalibrationComponent -> dart_vision_calibration + +dart_vision_core: + ros__parameters: + invert_image: false + exposure_time: 5000 + gain: 11.0 + L_H: 0.0 + L_S: 0.0 + L_V: 200.0 + U_H: 180.0 + U_S: 30.0 + U_V: 255.0 + enable_image_saving: false + image_save_directory: "./saved_images" + image_save_interval_ms: 1000 + save_raw_image: true + save_processed_image: false + +debug_display: + ros__parameters: + raw_image_topic: "/dart_guidance/raw_image" + processed_image_topic: "/dart_guidance/processed_image" + target_topic: "/dart_guidance/target_position" + display_raw: false + display_processed: true + max_fps: 30 + window_scale: 1.0 + save_on_error: false + save_directory: "./debug_saved" + +image_publisher: + ros__parameters: + Interface_name: "/dart_guidance/camera/display_image" + topic_name: "/dart_guidance/processed_image" + publish_freq: 15.0 + image_type: "mono8" + +camera_parameter_image_publisher: + ros__parameters: + Interface_name: "/dart_guidance/camera/camera_image" + topic_name: "/dart_guidance/camera/camera_image" + publish_freq: 30.0 + image_type: "bgr8" + +dart_vision_calibration: + ros__parameters: + image_topic: "/dart_guidance/camera/camera_image" + board_width: 7 + board_height: 7 + square_size: 0.006 + save_path: "./calibration_results" + enabled: true \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml index 58021bef..12777b5b 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml @@ -16,6 +16,10 @@ catapult_dart: usb_pid: -1 first_sample_spot: 1.0 final_sample_spot: 4.0 + trigger_runtime: 0x0000 + limiting_runtime: 0x0000 + lifting_left_runtime: 0x0000 + lifting_right_runtime: 0x0000 dart_settings: ros__parameters: @@ -38,22 +42,24 @@ launch_controller: b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 - lt_kp: 1.0 - lt_ki: 0.0 - lt_kd: 0.0 max_control_torque: 2.5 trigger_free_angle: 0x0E2A trigger_lock_angle: 0x0FC1 - lifting_up_angle: 0x0000 - lifting_middle_angle: 0x0000 - lifting_down_angle: 0x0000 - limiting_wait_time: 0x0000 - limiting_free_angle_: 0x0000 - limiting_lock_angle_: 0x0000 + lifting_up_angle_left: 0x06D7 + lifting_down_angle_left: 0x0DAA + lifting_up_angle_right: 0x08C9 + lifting_down_angle_right: 0x0C05 + limiting_wait_time: 0x0001 + limiting_free_angle_: 0x0001 + limiting_lock_angle_: 0x0001 actual_speed_and_control_torque_test_node: - ros__parameters: {} + ros__parameters: + lifting_up_angle_left: 0x06D7 + lifting_down_angle_left: 0x0DAA + lifting_up_angle_right: 0x08C9 + lifting_down_angle_right: 0x0B05 pitch_velocity_pid_controller: ros__parameters: @@ -85,7 +91,7 @@ force_pid_controller: Value_Broadcasters: ros__parameters: forward_list: - - "/dart/pitch/angle" + # - "/dart/pitch/angle" - "/dart/force_control_motor/velocity" - "/imu/catapult_roll_angle" - "/imu/catapult_pitch_angle" diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index cb9df80c..b897d695 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -3,42 +3,80 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::CatapultDart -> dart_hardware - - rmcs_core::controller::dart::DartLaunchController -> launch_controller - # - rmcs_core::hardware::ServoTest -> servo_test - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + # - rmcs_core::controller::dart::DartLaunchController -> launch_controller + - rmcs_core::hardware::ServoTest -> servo_test + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> force_pid_controller + # - rmcs_core::controller::dart::DartSettings -> dart_settings + # - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster -broadcaster: - ros__parameters: - forward_list: - - /dart/force_control_motor/control_velocity - - /dart/drive_belt/left/control_torque - - /dart/drive_belt/left/velocity - - /dart/drive_belt/right/control_torque - - /dart/drive_belt/right/velocity - - /dart/force_control_motor/velocity +# broadcaster: +# ros__parameters: +# forward_list: +# - /dart/force_control_motor/control_velocity +# - /dart/drive_belt/left/control_torque +# - /dart/drive_belt/left/velocity +# - /dart/drive_belt/right/control_torque +# - /dart/drive_belt/right/velocity +# - /dart/force_control_motor/velocity dart_hardware: ros__parameters: usb_pid: -1 + first_sample_spot: 1.0 + final_sample_spot: 4.0 -launch_controller: - ros__parameters: - belt_velocity: 10.0 - sync_coefficient: 0.5 - b_kp: 1.0 - b_ki: 0.0 - b_kd: 0.0 - max_control_torque: 1.0 - trigger_free_angle: 0x0E2A - trigger_lock_angle: 0x0FC1 +# launch_controller: +# ros__parameters: +# belt_velocity: 10.0 +# sync_coefficient: 0.5 +# b_kp: 1.0 +# b_ki: 0.0 +# b_kd: 0.0 +# max_control_torque: 1.0 +# trigger_free_angle: 0x0E2A +# trigger_lock_angle: 0x0FC1 servo_test: ros__parameters: trigger_free_angle: 0x0E2A trigger_lock_angle: 0x0FC1 + lifting_up_angle_left: 0x073E + lifting_middle_angle_left: 0x073E + lifting_down_angle_left: 0x0D18 + lifting_up_angle_right: 0x11DB + lifting_middle_angle_right: 0x11DB + lifting_down_angle_right: 0x0C52 + +# dart_settings: + # ros__parameters: + # log_enable: false + # screw_max_velocity: 10.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /dart/pitch_motor/velocity + setpoint: /pitch/control/velocity + control: /dart/pitch_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /dart/yaw_motor/velocity + setpoint: /yaw/control/velocity + control: /dart/yaw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 -dart_settings: +force_pid_controller: ros__parameters: - log_enable: false - screw_max_velocity: 10.0 \ No newline at end of file + measurement: /dart/force_control_motor/velocity + setpoint: /force/control/velocity + control: /dart/force_control_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 \ 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 3113c573..77acf075 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -116,8 +116,12 @@ Test plugin. - - + + Test plugin. + + + Test plugin. + Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp new file mode 100644 index 00000000..396972de --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* +launch controls +键位： +双下：全部停止 +双中：初始状态 + 此时{ + 右拨杆下拨再回中：切换上膛和退膛 + 处于上膛状态时右拨杆打到上：发射 + } +左拨杆上：设置模式 + 此时{ + 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 + 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 + } +*/ + +namespace rmcs_core::controller::dart { +class DartFillingTest + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartFillingTest() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , timer_interval_ms_(10) + , logger_(get_logger()) { + launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); + launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); + + lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); + lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); + + lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); + lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); + + limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); + limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); + limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/dart/lifting_left/current_angle", lifting_angle_left_); + register_input("/dart/lifting_right/current_angle", lifting_angle_right_); + + register_output("/dart/trigger_servo/control_angle", trigger_control_angle); + register_output("/dart/limiting_servo/control_angle", limiting_control_angle); + register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); + register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); + + register_output("/dart/filling/stage", filling_stage_); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms_), + [this] { timer_callback(); }); + + } + + void update() override { + using namespace rmcs_msgs; + + if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) + && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { + *launch_stage_ = DartLaunchStages::DISABLE; + *filling_stage_ = DartFillingStages::INIT; + reset_all_controls(); + + } else if (*switch_left_ == Switch::MIDDLE) { + + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { + *lifting_left_control_angle = lifting_down_angle_left_; + *lifting_right_control_angle = lifting_down_angle_right_; + } + + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { + *lifting_left_control_angle = lifting_down_angle_left_; + *lifting_right_control_angle = lifting_down_angle_right_; + } + + } else if (*switch_left_ == Switch::UP) { + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { + *trigger_control_angle = launch_lock_angle_; + *limiting_control_angle = limiting_lock_angle_; + } + + if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { + *trigger_control_angle = launch_trigger_angle_; + *limiting_control_angle = limiting_trigger_angle_; + } + } + last_switch_left_ = *switch_left_; + last_switch_right_ = *switch_right_; + last_launch_stage_ = *launch_stage_; + } + +private: + void reset_all_controls() { + *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + } + + void loading_process() { + *limiting_control_angle = limiting_trigger_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; + delay_and_execute(1000, [this]() { + *limiting_control_angle = limiting_lock_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; + }); + } + + rclcpp::TimerBase::SharedPtr timer_; + int timer_interval_ms_; + std::function delayed_action_; + bool is_delaying_ = false; + int delay_remaining_ms_ = 0; + + void timer_callback() { + if (is_delaying_ && delay_remaining_ms_ > 0) { + delay_remaining_ms_ -= timer_interval_ms_; + if (delay_remaining_ms_ <= 0) { + is_delaying_ = false; + if (delayed_action_) { + delayed_action_(); + } + } + } + } + + void delay_and_execute(int delay_ms, std::function action) { + if (!is_delaying_) { + is_delaying_ = true; + delay_remaining_ms_ = delay_ms; + delayed_action_ = std::move(action); + } + } + + rclcpp::Logger logger_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + rmcs_msgs::Switch last_switch_right_; + rmcs_msgs::Switch last_switch_left_; + + InputInterface lifting_angle_left_; + InputInterface lifting_angle_right_; + + OutputInterface launch_stage_; + rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + + uint16_t launch_lock_angle_; + uint16_t launch_trigger_angle_; + OutputInterface trigger_control_angle; + + OutputInterface filling_stage_; + + uint16_t lifting_up_angle_left_; + uint16_t lifting_down_angle_left_; + uint16_t lifting_up_angle_right_; + uint16_t lifting_down_angle_right_; + + OutputInterface lifting_left_control_angle; + OutputInterface lifting_right_control_angle; + + uint16_t limiting_lock_angle_; + uint16_t limiting_trigger_angle_; + uint16_t limiting_wait_time_; + + OutputInterface limiting_control_angle; + +}; + +} // namespace rmcs_core::controller::dart + +#include +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFillingTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp index 33ad4e53..88d50695 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp @@ -41,9 +41,9 @@ class DartSettings register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); - register_input("/dart/pitch_motor/angle", pitch_angle_); register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); + // register_input("/dart_guidance/angle/error", yaw_pitch_angle_); register_output("/yaw/control/velocity", yaw_control_velocity_); register_output("/force/control/velocity", force_control_); register_output("/pitch/control/velocity", pitch_control_velocity_); @@ -86,7 +86,8 @@ class DartSettings *pitch_control_velocity_ = joystick_right_->x() * max_transform_rate_; break; } else if (is_auto_pitch_control_mode_ == 1){ // double loop pid - *yaw_control_velocity_ = joystick_left_->y() * max_transform_rate_; + // double yaw_angle = (*yaw_pitch_angle_)[0]; + // *yaw_control_velocity_ = yaw_angle * max_transform_rate_; pitch_control(); break; } @@ -125,7 +126,8 @@ class DartSettings pitch_angle_pid_controller.kp = pitch_angle_kp_; pitch_angle_pid_controller.ki = pitch_angle_ki_; pitch_angle_pid_controller.kd = pitch_angle_kd_; - *pitch_control_velocity_ = pitch_angle_pid_controller.update(pitch_angle_setpoint_ - *pitch_angle_); + // double pitch_angle = (*yaw_pitch_angle_)[1]; + // *pitch_control_velocity_ = pitch_angle_pid_controller.update(pitch_angle_setpoint_ - pitch_angle); } void force_control() { @@ -162,9 +164,9 @@ class DartSettings InputInterface switch_right_; InputInterface switch_left_; - InputInterface pitch_angle_; InputInterface force_sensor_ch1_data_; InputInterface force_sensor_ch2_data_; + // InputInterface yaw_pitch_angle_; OutputInterface yaw_control_velocity_; OutputInterface force_control_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp index 2cae5a67..098d1542 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp @@ -1,3 +1,6 @@ +#include +#include +#include #include #include #include @@ -11,28 +14,236 @@ class Test Test() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) { - register_input("/dart/pitch_motor/control_torque", pitch_control_torque_); - register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); - register_input("/dart/pitch_motor/velocity", pitch_speed_); - register_input("/dart/yaw_motor/velocity", yaw_speed_); + register_input("/dart/pitch_motor/control_torque", pitch_control_torque_); + register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); + register_input("/dart/pitch_motor/velocity", pitch_speed_); + register_input("/dart/yaw_motor/velocity", yaw_speed_); register_input("/imu/catapult_roll_angle", final_roll_); register_input("/imu/catapult_pitch_angle", final_pitch_); register_input("/imu/catapult_yaw_angle", final_yaw_); register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_);} + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); + register_input("/dart/lifting_left/current_angle", lifting_angle_left_); + register_input("/dart/lifting_right/current_angle", lifting_angle_right_); + register_input("/dart/lifting_left/control_angle", lifting_control_angle_left_); + register_input("/dart/lifting_right/control_angle", lifting_control_angle_right_); + + lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); + lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); + lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); + lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); + + declare_parameter("fit_window_size", 50); + declare_parameter("fit_threshold", 1.0); + declare_parameter("stable_min_points", 20); + declare_parameter("stable_region_timeout", 500); + + fit_window_size_ = static_cast(get_parameter("fit_window_size").as_int()); + fit_threshold_ = get_parameter("fit_threshold").as_double(); + stable_min_points_ = static_cast(get_parameter("stable_min_points").as_int()); + stable_region_timeout_ = static_cast(get_parameter("stable_region_timeout").as_int()); + + total_query_pairs_ = 0; + sync_query_pairs_ = 0; + left_has_pending_ = false; + right_has_pending_ = false; + left_pending_delta_ = 0; + right_pending_delta_ = 0; + } + void update() override { - if (count_n == 100) { - // RCLCPP_INFO(this->get_logger(), "pitch_control_torque = %f, yaw_control_torque = %f", *pitch_control_torque_, *yaw_control_torque_); - // RCLCPP_INFO(this->get_logger(), "pitch_speed = %f, yaw_speed = %f", *pitch_speed_, *yaw_speed_); - RCLCPP_INFO(this->get_logger(), "final_roll = %f, final_pitch = %f, final_yaw = %f", *final_roll_, *final_pitch_, *final_yaw_); - // RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); - count_n = 0; + // if (count_n % 100 == 0) { + // RCLCPP_INFO(logger_, "left_force: %d, right_force: %d", + // *force_sensor_ch1_data_, *force_sensor_ch2_data_); + // } + // count_n++; + if (*lifting_angle_left_ != last_left_angle_for_sync_) { + int16_t delta = calculateAngleDelta(last_left_angle_for_sync_, *lifting_angle_left_); + last_left_angle_for_sync_ = *lifting_angle_left_; + left_has_pending_ = true; + left_pending_delta_ = delta; + } + + if (*lifting_angle_right_ != last_right_angle_for_sync_) { + int16_t delta = calculateAngleDelta(last_right_angle_for_sync_, *lifting_angle_right_); + last_right_angle_for_sync_ = *lifting_angle_right_; + right_has_pending_ = true; + right_pending_delta_ = delta; + } + + if (left_has_pending_ && right_has_pending_) { + total_query_pairs_++; + int16_t delta_diff = std::abs(left_pending_delta_ - right_pending_delta_); + if (delta_diff <= 3) { + sync_query_pairs_++; + } else { + RCLCPP_WARN(logger_, "Lift sync mismatch: left delta=%d, right delta=%d, diff=%d", + left_pending_delta_, right_pending_delta_, delta_diff); + } + left_has_pending_ = false; + right_has_pending_ = false; } + + recordAngleToWindow(); + + checkStableRegion(); + + if (is_in_stable_region_) { + calculateVibrationInStableRegion(); + } + + if (count_n % 1000 == 0) { + syn = (count_n > 1) ? (double)sync_coefficient_count_ / (count_n - 1) : 0.0; + double sync_ratio = (total_query_pairs_ > 0) ? (double)sync_query_pairs_ / total_query_pairs_ : 0.0; + + if (total_stable_points_ > 0) { + vib = (double)stable_at_control_angle_count_ / total_stable_points_; + RCLCPP_INFO(logger_, + "left_angle=%d, right_angle=%d | " + "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=%.4f", + *lifting_angle_left_, *lifting_angle_right_, + syn, sync_ratio, sync_query_pairs_, total_query_pairs_, vib); + } else { + RCLCPP_INFO(logger_, + "left_angle=%d, right_angle=%d | " + "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=N/A", + *lifting_angle_left_, *lifting_angle_right_, + syn, sync_ratio, sync_query_pairs_, total_query_pairs_); + } + } + count_n++; } + private: - int count_n = 0; + static int16_t calculateAngleDelta(uint16_t last_angle, uint16_t current_angle) { + int16_t delta = static_cast(current_angle - last_angle); + + if (delta > 32767) { + delta -= static_cast(65536); + } else if (delta < -32768) { + delta += static_cast(65536); + } + + return delta; + } + + void recordAngleToWindow() { + angle_window_.push_back(*lifting_angle_left_); + if (angle_window_.size() > fit_window_size_) { + angle_window_.pop_front(); + } + } + + void checkStableRegion() { + if (angle_window_.size() < stable_min_points_) { + stable_region_timeout_counter_ = 0; + return; + } + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + size_t n = angle_window_.size(); + + for (size_t i = 0; i < n; ++i) { + double x = static_cast(i); + double y = static_cast(angle_window_[i]); + sum_x += x; + sum_y += y; + sum_xy += x * y; + sum_xx += x * x; + } + + double b = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x); + double a = (sum_y - b * sum_x) / n; + + double residual_sum = 0.0; + for (size_t i = 0; i < n; ++i) { + double x = static_cast(i); + double y = static_cast(angle_window_[i]); + double y_fit = a + b * x; + residual_sum += (y - y_fit) * (y - y_fit); + } + + double rms_error = sqrt(residual_sum / n); + + bool is_stable = (std::abs(b) < 0.05 && rms_error < fit_threshold_); + + if (is_stable) { + if (!is_in_stable_region_) { + is_in_stable_region_ = true; + current_stable_start_point_ = angle_window_[angle_window_.size() - 1]; + stable_region_timeout_counter_ = 0; + + // RCLCPP_DEBUG(logger_, "Enter stable region: slope=%.4f, rms=%.4f", b, rms_error); + } + } else { + if (is_in_stable_region_) { + stable_region_timeout_counter_++; + if (stable_region_timeout_counter_ > stable_region_timeout_) { + is_in_stable_region_ = false; + stable_region_timeout_counter_ = 0; + + if (current_stable_total_points_ > 0) { + double stable_ratio = static_cast(current_stable_at_control_angle_) / + current_stable_total_points_; + // RCLCPP_DEBUG(logger_, "Exit stable region: total=%d, at control=%d, ratio=%.4f", + // current_stable_total_points_, current_stable_at_control_angle_, stable_ratio); + } + + current_stable_total_points_ = 0; + current_stable_at_control_angle_ = 0; + } + } + } + } + + void calculateVibrationInStableRegion() { + current_stable_total_points_++; + if (*lifting_angle_left_ == *lifting_control_angle_left_ && + *lifting_angle_right_ == *lifting_control_angle_right_) { + current_stable_at_control_angle_++; + } + + total_stable_points_++; + if (*lifting_angle_left_ == *lifting_control_angle_left_ && + *lifting_angle_right_ == *lifting_control_angle_right_) { + stable_at_control_angle_count_++; + } + } + +private: + int count_n = 1; + int sync_coefficient_count_ = 0; rclcpp::Logger logger_; + double syn = 0.0; + double vib = 0.0; + + uint16_t last_left_angle_for_sync_ = 0; + uint16_t last_right_angle_for_sync_ = 0; + bool left_has_pending_; + bool right_has_pending_; + int16_t left_pending_delta_; + int16_t right_pending_delta_; + int64_t total_query_pairs_; + int64_t sync_query_pairs_; + + std::deque angle_window_; + + bool is_in_stable_region_ = false; + int stable_region_timeout_counter_ = 0; + uint16_t current_stable_start_point_ = 0; + + int current_stable_total_points_ = 0; + int current_stable_at_control_angle_ = 0; + + int total_stable_points_ = 0; + int stable_at_control_angle_count_ = 0; + + int fit_window_size_ = 50; + double fit_threshold_ = 1.0; + int stable_min_points_ = 20; + int stable_region_timeout_ = 500; + InputInterface pitch_control_torque_; InputInterface yaw_control_torque_; InputInterface pitch_speed_; @@ -42,7 +253,17 @@ class Test InputInterface final_yaw_; InputInterface force_sensor_ch1_data_; InputInterface force_sensor_ch2_data_; - + InputInterface lifting_angle_left_; + InputInterface lifting_angle_right_; + InputInterface lifting_control_angle_left_; + InputInterface lifting_control_angle_right_; + + uint16_t lifting_up_angle_left_; + uint16_t lifting_middle_angle_left_; + uint16_t lifting_down_angle_left_; + uint16_t lifting_up_angle_right_; + uint16_t lifting_middle_angle_right_; + uint16_t lifting_down_angle_right_; }; } // namespace rmcs_core::controller::dart diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 7b8504a0..3adf136b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -36,27 +36,24 @@ class DartLaunchController : Node{ get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , timer_interval_ms_(10) , logger_(get_logger()) , drive_belt_pid_calculator_( get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()) - , lifting_table_pid_calculator_( - get_parameter("lt_kp").as_double(), get_parameter("lt_ki").as_double(), - get_parameter("lt_kd").as_double()) { + get_parameter("b_kd").as_double()) { - dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); + drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); sync_coefficient_ = get_parameter("sync_coefficient").as_double(); max_control_torque_ = get_parameter("max_control_torque").as_double(); - lifting_table_working_velocity_ = get_parameter("lifting_velocity").as_double(); - lifting_sync_coefficient_ = get_parameter("lifting_sync_coefficient").as_double(); - launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - lifting_up_angle_ = get_parameter("lifting_up_angle").as_int(); - lifting_middle_angle_ = get_parameter("lifting_middle_angle").as_int(); - lifting_down_angle_ = get_parameter("lifting_down_angle").as_int(); + lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); + lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); + + lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); + lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); @@ -70,13 +67,25 @@ class DartLaunchController register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + register_input("/dart/lifting_left/current_angle", lifting_angle_left_); + register_input("/dart/lifting_right/current_angle", lifting_angle_right_); + register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - register_output("/dart/lifting_servo/control_angle", lifting_contorl_angle); - register_output("/dart/limiting_servo/control_angle", limiting_contorl_angle); + register_output("/dart/limiting_servo/control_angle", limiting_control_angle); + register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); + register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); + + register_output("/dart/filling/stage", filling_stage_); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms_), + [this] { timer_callback(); }); + // *lifting_left_control_angle = lifting_up_angle_left_; + // *lifting_right_control_angle = lifting_up_angle_right_; } void update() override { @@ -85,26 +94,36 @@ class DartLaunchController if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { *launch_stage_ = DartLaunchStages::DISABLE; + *filling_stage_ = DartFillingStages::INIT; reset_all_controls(); } else if (*switch_left_ == Switch::MIDDLE) { if (last_launch_stage_ == DartLaunchStages::DISABLE) { *launch_stage_ = DartLaunchStages::RESETTING; + // *filling_stage_ = DartFillingStages::FILLING; // assume that we already have a dart on the lifting platform at the beginning } if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { if (last_launch_stage_ == DartLaunchStages::INIT) { *launch_stage_ = DartLaunchStages::LOADING; - lifting_table_position_flag_ = 1; } else if (last_launch_stage_ == DartLaunchStages::READY) { + *lifting_left_control_angle = lifting_up_angle_left_; + *lifting_right_control_angle = lifting_up_angle_right_; *launch_stage_ = DartLaunchStages::CANCEL; } } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { if (last_launch_stage_ == DartLaunchStages::READY) { *launch_stage_ = DartLaunchStages::INIT; trigger_lock_flag_ = false; - loading_process(); + delay_and_execute(20, [this]() { + *lifting_left_control_angle = lifting_up_angle_left_; + *lifting_right_control_angle = lifting_up_angle_right_; + *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; + }); + delay_and_execute(500, [this]() { + loading_process(); + }); } else { RCLCPP_INFO(logger_, "Dart has't been loaded !"); } @@ -112,58 +131,66 @@ class DartLaunchController if (blocking_detection()) { if (last_launch_stage_ == DartLaunchStages::LOADING) { - *launch_stage_ = DartLaunchStages::RESETTING; - lifting_table_position_flag_ = 2; - trigger_lock_flag_ = true; - dirve_belt_block_count_ = 0; - + *lifting_left_control_angle = lifting_down_angle_left_; + *lifting_right_control_angle = lifting_down_angle_right_; + *filling_stage_ = rmcs_msgs::DartFillingStages::DOWNING; + delay_and_execute(500, [this]() { + *launch_stage_ = DartLaunchStages::RESETTING; + *filling_stage_ = rmcs_msgs::DartFillingStages::READY; + trigger_lock_flag_ = true; + drive_belt_block_count_ = 0; + }); + // if (*lifting_angle_left_ == lifting_down_angle_left_ && *lifting_angle_right_ == lifting_down_angle_right_) { + + // *launch_stage_ = DartLaunchStages::RESETTING; + // *filling_stage_ = rmcs_msgs::DartFillingStages::READY; + // trigger_lock_flag_ = true; + // drive_belt_block_count_ = 0; + // } } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { - *launch_stage_ = DartLaunchStages::RESETTING; - lifting_table_position_flag_ = 1; - trigger_lock_flag_ = false; - dirve_belt_block_count_ = 0; - + *lifting_left_control_angle = lifting_up_angle_left_; + *lifting_right_control_angle = lifting_up_angle_right_; + *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; + delay_and_execute(500, [this]() { + *launch_stage_ = DartLaunchStages::RESETTING; + *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; + trigger_lock_flag_ = false; + drive_belt_block_count_ = 0; + }); + // if (*lifting_angle_left_ == lifting_up_angle_left_ && *lifting_angle_right_ == lifting_up_angle_right_) { + // *launch_stage_ = DartLaunchStages::RESETTING; + // *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; + // trigger_lock_flag_ = false; + // drive_belt_block_count_ = 0; + // }; } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { *launch_stage_ = trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; - dirve_belt_block_count_ = 0; + *filling_stage_ = + trigger_lock_flag_ ? DartFillingStages::READY : DartFillingStages::INIT; + drive_belt_block_count_ = 0; } } double control_velocity = 0; if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { - control_velocity = -dirve_belt_working_velocity_; + control_velocity = -drive_belt_working_velocity_; } else if ( *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { - control_velocity = dirve_belt_working_velocity_; + control_velocity = drive_belt_working_velocity_; } else { control_velocity = 0; } drive_belt_sync_control(control_velocity); - RCLCPP_INFO(logger_, "%lf", control_velocity); + // RCLCPP_INFO(logger_, "%lf", control_velocity); } *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; - *limiting_contorl_angle = limiting_lock_flag_ ? limiting_lock_angle_ : limiting_trigger_angle_; - switch(lifting_table_position_flag_){ - case 0: *lifting_contorl_angle = lifting_up_angle_; - break; - case 1: *lifting_contorl_angle = lifting_middle_angle_; - break; - case 2: *lifting_contorl_angle = lifting_down_angle_; - break; - default: break; - } last_switch_left_ = *switch_left_; last_switch_right_ = *switch_right_; last_launch_stage_ = *launch_stage_; - // RCLCPP_INFO(logger_, "%lf | %lf", *right_drive_belt_velocity_, - // *left_drive_belt_velocity_); - // RCLCPP_INFO( - // logger_, "%d | %lf | %lf", static_cast(*launch_stage_), - // *left_drive_belt_control_torque_, *right_drive_belt_control_torque_); } private: @@ -199,20 +226,49 @@ class DartLaunchController if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) || (abs(*right_drive_belt_velocity_) < 0.5 && abs(*right_drive_belt_control_torque_) > 0.5)) { - dirve_belt_block_count_++; + drive_belt_block_count_++; } - return dirve_belt_block_count_ > 1000 ? true : false; + return drive_belt_block_count_ > 1000 ? true : false; } void loading_process() { - lifting_table_position_flag_ = 0; + *limiting_control_angle = limiting_trigger_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; + delay_and_execute(2000, [this]() { + *limiting_control_angle = limiting_lock_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; + }); } - int dirve_belt_block_count_ = 0; - double dirve_belt_working_velocity_; + rclcpp::TimerBase::SharedPtr timer_; + int timer_interval_ms_; + std::function delayed_action_; + bool is_delaying_ = false; + int delay_remaining_ms_ = 0; + + void timer_callback() { + if (is_delaying_ && delay_remaining_ms_ > 0) { + delay_remaining_ms_ -= timer_interval_ms_; + if (delay_remaining_ms_ <= 0) { + is_delaying_ = false; + if (delayed_action_) { + delayed_action_(); + } + } + } + } - double lifting_table_working_velocity_; + void delay_and_execute(int delay_ms, std::function action) { + if (!is_delaying_) { + is_delaying_ = true; + delay_remaining_ms_ = delay_ms; + delayed_action_ = std::move(action); + } + } + + int drive_belt_block_count_ = 0; + double drive_belt_working_velocity_; rclcpp::Logger logger_; @@ -223,6 +279,8 @@ class DartLaunchController rmcs_msgs::Switch last_switch_right_; rmcs_msgs::Switch last_switch_left_; + InputInterface lifting_angle_left_; + InputInterface lifting_angle_right_; OutputInterface left_drive_belt_control_torque_; OutputInterface right_drive_belt_control_torque_; InputInterface left_drive_belt_velocity_; @@ -238,28 +296,30 @@ class DartLaunchController bool trigger_lock_flag_ = false; OutputInterface trigger_control_angle; - uint16_t lifting_up_angle_; - uint16_t lifting_middle_angle_; - uint16_t lifting_down_angle_; - int lifting_table_position_flag_ = 0; //0: up, 1: middle, 2:down - OutputInterface lifting_contorl_angle; + OutputInterface filling_stage_; + OutputInterface pulse_sending_flag_; + + uint16_t lifting_up_angle_left_; + uint16_t lifting_down_angle_left_; + uint16_t lifting_up_angle_right_; + uint16_t lifting_down_angle_right_; + + OutputInterface lifting_left_control_angle; + OutputInterface lifting_right_control_angle; uint16_t limiting_lock_angle_; uint16_t limiting_trigger_angle_; uint16_t limiting_wait_time_; - bool limiting_lock_flag_ = false; - OutputInterface limiting_contorl_angle; + OutputInterface limiting_control_angle; pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; double sync_coefficient_; - - pid::MatrixPidCalculator<2> lifting_table_pid_calculator_; - double lifting_sync_coefficient_; }; } // namespace rmcs_core::controller::dart #include +#include PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp index 9a2f48cc..e89f6707 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp @@ -15,21 +15,41 @@ class ServoTest , logger_(get_logger()) { trigger_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); trigger_free_angle_ = get_parameter("trigger_free_angle").as_int(); + lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); + lifting_middle_angle_left_ = get_parameter("lifting_middle_angle_left").as_int(); + lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); + lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); + lifting_middle_angle_right_ = get_parameter("lifting_middle_angle_right").as_int(); + lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); register_input("/remote/switch/left", switch_left_); register_output("/dart/trigger_servo/control_angle", trigger_control_angle); + register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); + register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); + } void update() override { using namespace rmcs_msgs; + + *trigger_control_angle = trigger_lock_angle_; + if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { - *trigger_control_angle = trigger_lock_angle_; + *lifting_left_control_angle = lifting_down_angle_left_; + *lifting_right_control_angle = lifting_down_angle_right_; } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { - *trigger_control_angle = trigger_free_angle_; + *lifting_left_control_angle = lifting_up_angle_left_; + *lifting_right_control_angle = lifting_up_angle_right_; } + + // if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { + // *trigger_control_angle = trigger_lock_angle_; + // } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { + // *trigger_control_angle = trigger_free_angle_; + // } } private: @@ -43,6 +63,17 @@ class ServoTest uint16_t trigger_lock_angle_; uint16_t trigger_free_angle_; OutputInterface trigger_control_angle; + OutputInterface lifting_left_control_angle; + OutputInterface lifting_right_control_angle; + + uint16_t lifting_up_angle_left_; + uint16_t lifting_middle_angle_left_; + uint16_t lifting_down_angle_left_; + uint16_t lifting_up_angle_right_; + uint16_t lifting_middle_angle_right_; + uint16_t lifting_down_angle_right_; + + }; } // namespace rmcs_core::hardware diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 99e84632..0ba79505 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -12,12 +12,14 @@ #include #include "filter/low_pass_filter.hpp" #include +#include #include #include #include #include #include #include +#include namespace rmcs_core::hardware { @@ -58,19 +60,18 @@ class CatapultDart .set_reduction_ratio(19.) .set_reversed()}) , force_sensor_(*this) - , trigger_servo_(*dart_command_, "/dart/trigger_servo") - , elevating_left_(*dart_command_, "/dart/lifting_left") - , elevating_right_(*dart_command_, "/dart/lifting_right") - , limiting_servo_(*dart_command_, "/dart/limiting_servo") + , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) + , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) + , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) + , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) , transmit_buffer_(*this, 32) , event_thread_([this]() { handle_events(); }) { - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - register_output("/elevating/angle", elevating_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); + register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); @@ -79,10 +80,37 @@ class CatapultDart imu_sampler_initialize(); tf_broadcaster_ = std::make_unique(*this); start_time_ = std::chrono::steady_clock::now(); + trigger_calibrate_subscription_ = create_subscription( "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(std::move(msg)); + trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); + }); + limiting_calibrate_subscription_ = create_subscription( + "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); + }); + lifting_left_calibrate_subscription_ = create_subscription( + "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr); }); + lifting_right_calibrate_subscription_ = create_subscription( + "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr); + }); + + 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; + }; + + last_read_left_time_ = this->now(); + last_read_right_time_ = this->now(); } ~CatapultDart() override { @@ -92,7 +120,6 @@ class CatapultDart void update() override { dr16_.update_status(); - pitch_motor_.update_status(); yaw_motor_.update_status(); drive_belt_motor_[0].update_status(); @@ -126,22 +153,74 @@ class CatapultDart transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); if (!trigger_servo_.calibrate_mode()) { + uint16_t current_target = trigger_servo_.get_target_angle(); + if (current_target != last_trigger_angle_) { + size_t uart_data_length; + std::unique_ptr command_buffer = + trigger_servo_.generate_runtime_command(uart_data_length); + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); + last_trigger_angle_ = current_target; + } + } + + if (!limiting_servo_.calibrate_mode()) { + uint16_t current_target = limiting_servo_.get_target_angle(); + if (current_target != last_limiting_angle_) { + size_t uart_data_length; + auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_limiting_angle_ = current_target; + } + } + + if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { + uint16_t current_target_left = lifting_left_.get_target_angle(); + uint16_t current_target_right = lifting_right_.get_target_angle(); + if (current_target_left != last_lifting_left_angle_) { + size_t uart_data_length; + uint16_t runtime_left = 0; + uint16_t runtime_right = 0; + std::unique_ptr command_buffer = + device::TriggerServo::generate_sync_run_command(uart_data_length, + LIFTING_LEFT_ID, LIFTING_RIGHT_ID, + current_target_left, current_target_right, + runtime_left, runtime_right); + const auto lifting_table_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); + last_lifting_left_angle_ = current_target_left; + } + } + + + auto now = this->now(); + + if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { size_t uart_data_length; - std::unique_ptr command_buffer = - trigger_servo_.generate_runtime_command(uart_data_length); - const auto trigger_servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); + auto command_buffer = lifting_left_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_read_left_time_ = now; + } + if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) { + if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { + size_t uart_data_length; + auto command_buffer = lifting_right_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_read_right_time_ = now; + } } transmit_buffer_.trigger_transmission(); } + int pub_time_count_ = 0; 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; @@ -169,56 +248,122 @@ class CatapultDart } } - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); + void uart1_receive_callback(const std::byte* data, uint8_t length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&data](std::byte* storage) { *storage = *data++; }, length); } void uart2_receive_callback(const std::byte* data, uint8_t length) override { - bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); - if (!success) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + std::string hex_string = bytes_to_hex_string(data, length); + RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, hex_string.c_str()); + + if (length < 3) { + RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); + return; + } + + uint8_t servo_id = static_cast(data[2]); + std::pair result{false, 0}; + + switch (servo_id) { + case TRIGGER_SERVO_ID: + result = trigger_servo_.calibrate_current_angle(logger_, data, length); + if (!result.first) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIMITING_SERVO_ID: + result = limiting_servo_.calibrate_current_angle(logger_, data, length); + if (!result.first) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIFTING_LEFT_ID: + result = lifting_left_.calibrate_current_angle(logger_, data, length); + if (result.first) { + *lifting_current_angle_left_ = result.second; + } else { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIFTING_RIGHT_ID: + result = lifting_right_.calibrate_current_angle(logger_, data, length); + if (result.first) { + *lifting_current_angle_right_ = result.second; + } else { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + break; } + } 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 { + imu_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); + } + private: - void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { - trigger_servo_.set_calibrate_mode(msg->data); + static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; + static constexpr uint8_t LIMITING_SERVO_ID = 0x02; + static constexpr uint8_t LIFTING_LEFT_ID = 0x03; + static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; + std::byte * trigger_servo_uart_data_ptr; + std::byte * limiting_servo_uart_data_ptr; + std::byte * lifting_left_uart_data_ptr; + std::byte * lifting_right_uart_data_ptr; + + void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ + , std_msgs::msg::Int32::UniquePtr msg + , std::byte* servo_uart_data_ptr) { + /* + 标定命令格式： + ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" + 替换data值就行 + */ + servo_.set_calibrate_mode(msg->data); std::unique_ptr command_buffer; size_t command_length = 0; if (msg->data == 0) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); } else if (msg->data == 1) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); } else if (msg->data == 2) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); } else if (msg->data == 3) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); } else if (msg->data == 4) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); } else if (msg->data == 5) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); } else if (msg->data == 6) { - command_buffer = trigger_servo_.generate_calibrate_command( - device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); } - const auto trigger_servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); + servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); + + std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); + RCLCPP_INFO( + this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, + hex_string.c_str()); } static std::string bytes_to_hex_string(const std::byte* data, size_t size) { @@ -238,7 +383,7 @@ class CatapultDart result.pop_back(); } return result; - } + } // DEBUG TOOL void setup_imu_coordinate_mapping() { imu_.set_coordinate_mapping( @@ -250,7 +395,6 @@ class CatapultDart void imu_sampler_initialize() { start_time_ = std::chrono::steady_clock::now(); yaw_drift_coefficient_ = 0.0; - } void processImuData() { @@ -258,7 +402,6 @@ class CatapultDart double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); @@ -273,7 +416,7 @@ class CatapultDart yaw_samples_.push_back(transformed_yaw); time_samples_.push_back(elapsed_seconds); double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = yaw_samples_.size(); + double n = static_cast(yaw_samples_.size()); for (int i = 0; i < n; ++i) { sum_x += time_samples_[i]; @@ -322,94 +465,84 @@ class CatapultDart r.x = x; r.y = y; r.z = z; r.w = w; return r; }; + geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - // Create quaternions for each axis rotation Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - - // Combined rotation: yaw -> pitch -> roll Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - // Publish all TF transforms tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", zero_trans, create_rotation(0, 0, 0, 1))); - tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), yaw_quat.z(), yaw_quat.w()))); - tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), pitch_quat.z(), pitch_quat.w()))); - tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), combined_quaternion.z(), combined_quaternion.w()))); - tf_broadcaster_->sendTransform(create_transform("world", "base_link", zero_trans, create_rotation(0, 0, 0, 1))); } rclcpp::Logger logger_; - filter::LowPassFilter<1> pitch_angle_filter_; filter::LowPassFilter<1> pitch_velocity_filter_; filter::LowPassFilter<1> yaw_velocity_filter_; filter::LowPassFilter<1> yaw_filter_; filter::LowPassFilter<1> pitch_filter_; filter::LowPassFilter<1> roll_filter_; - std::chrono::steady_clock::time_point start_time_; - class DartCommand : public rmcs_executor::Component { public: - explicit DartCommand(CatapultDart& robot) - : dart_(robot) {} - + explicit DartCommand(CatapultDart& robot) : dart_(robot) {} void update() override { dart_.command_update(); } - CatapultDart& dart_; }; + std::shared_ptr dart_command_; double first_sample_spot_; double final_sample_spot_; - OutputInterface tf_; std::unique_ptr tf_broadcaster_; - device::Dr16 dr16_; device::Bmi088 imu_; device::DjiMotor pitch_motor_; device::DjiMotor yaw_motor_; device::DjiMotor force_control_motor_; device::DjiMotor drive_belt_motor_[2]; - device::ForceSensorRuntime force_sensor_; device::TriggerServo trigger_servo_; - device::TriggerServo elevating_left_; - device::TriggerServo elevating_right_; device::TriggerServo limiting_servo_; - + device::TriggerServo lifting_left_; + device::TriggerServo lifting_right_; rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; - + rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; + rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; + rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; librmcs::client::CBoard::TransmitBuffer transmit_buffer_; std::thread event_thread_; - OutputInterface catapult_pitch_angle_; OutputInterface catapult_roll_angle_; OutputInterface catapult_yaw_angle_; - OutputInterface yaw_samples_output_; - OutputInterface elevating_angle_; - + OutputInterface lifting_current_angle_left_; + OutputInterface lifting_current_angle_right_; double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_, time_samples_; + uint16_t last_trigger_angle_ = 0xFFFF; + uint16_t last_limiting_angle_ = 0xFFFF; + uint16_t last_lifting_left_angle_ = 0xFFFF; + rclcpp::Time last_read_left_time_; + rclcpp::Time last_read_right_time_; + const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); }; } // namespace rmcs_core::hardware #include - PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp new file mode 100644 index 00000000..5e1382bf --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp @@ -0,0 +1,316 @@ +#include "librmcs/client/cboard.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/trigger_servo.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware { + +class DartFillingTestHardware + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + DartFillingTestHardware() + : 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()) + , dart_command_(create_partner_component(get_component_name() + "_command", *this)) + , dr16_(*this) + , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) + , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) + , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) + , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); + register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); + + trigger_calibrate_subscription_ = create_subscription( + "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); + }); + limiting_calibrate_subscription_ = create_subscription( + "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); + }); + lifting_left_calibrate_subscription_ = create_subscription( + "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr); + }); + lifting_right_calibrate_subscription_ = create_subscription( + "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr); + }); + + 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; + }; + + last_read_left_time_ = this->now(); + last_read_right_time_ = this->now(); + } + + ~DartFillingTestHardware() override { + stop_handling_events(); + event_thread_.join(); + } + + void update() override { + dr16_.update_status(); + } + + void command_update() { + + if (!trigger_servo_.calibrate_mode()) { + uint16_t current_target = trigger_servo_.get_target_angle(); + if (current_target != last_trigger_angle_) { + size_t uart_data_length; + std::unique_ptr command_buffer = + trigger_servo_.generate_runtime_command(uart_data_length); + const auto trigger_servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); + last_trigger_angle_ = current_target; + } + } + + if (!limiting_servo_.calibrate_mode()) { + uint16_t current_target = limiting_servo_.get_target_angle(); + if (current_target != last_limiting_angle_) { + size_t uart_data_length; + auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_limiting_angle_ = current_target; + } + } + + if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { + uint16_t current_target_left = lifting_left_.get_target_angle(); + uint16_t current_target_right = lifting_right_.get_target_angle(); + if (current_target_left != last_lifting_left_angle_) { + size_t uart_data_length; + uint16_t runtime_left = 0; + uint16_t runtime_right = 0; + std::unique_ptr command_buffer = + device::TriggerServo::generate_sync_run_command(uart_data_length, + LIFTING_LEFT_ID, LIFTING_RIGHT_ID, + current_target_left, current_target_right, + runtime_left, runtime_right); + const auto lifting_table_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); + last_lifting_left_angle_ = current_target_left; + } + } + + + auto now = this->now(); + + if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { + size_t uart_data_length; + auto command_buffer = lifting_left_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_read_left_time_ = now; + } + if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) { + if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { + size_t uart_data_length; + auto command_buffer = lifting_right_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); + transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); + last_read_right_time_ = now; + } + } + + transmit_buffer_.trigger_transmission(); + } + + int pub_time_count_ = 0; + +protected: + void uart1_receive_callback(const std::byte* data, uint8_t length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&data](std::byte* storage) { *storage = *data++; }, length); + } + + void uart2_receive_callback(const std::byte* data, uint8_t length) override { + std::string hex_string = bytes_to_hex_string(data, length); + RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, hex_string.c_str()); + + if (length < 3) { + RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); + return; + } + + uint8_t servo_id = static_cast(data[2]); + std::pair result{false, 0}; + + switch (servo_id) { + case TRIGGER_SERVO_ID: + result = trigger_servo_.calibrate_current_angle(logger_, data, length); + if (!result.first) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIMITING_SERVO_ID: + result = limiting_servo_.calibrate_current_angle(logger_, data, length); + if (!result.first) { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIFTING_LEFT_ID: + result = lifting_left_.calibrate_current_angle(logger_, data, length); + if (result.first) { + *lifting_current_angle_left_ = result.second; + } else { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + case LIFTING_RIGHT_ID: + result = lifting_right_.calibrate_current_angle(logger_, data, length); + if (result.first) { + *lifting_current_angle_right_ = result.second; + } else { + RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + } + break; + default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); + break; + } + + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + +private: + static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; + static constexpr uint8_t LIMITING_SERVO_ID = 0x02; + static constexpr uint8_t LIFTING_LEFT_ID = 0x03; + static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; + std::byte * trigger_servo_uart_data_ptr; + std::byte * limiting_servo_uart_data_ptr; + std::byte * lifting_left_uart_data_ptr; + std::byte * lifting_right_uart_data_ptr; + + void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ + , std_msgs::msg::Int32::UniquePtr msg + , std::byte* servo_uart_data_ptr) { + /* + 标定命令格式： + ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" + 替换data值就行 + */ + servo_.set_calibrate_mode(msg->data); + + std::unique_ptr command_buffer; + size_t command_length = 0; + if (msg->data == 0) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); + } else if (msg->data == 1) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); + } else if (msg->data == 2) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); + } else if (msg->data == 3) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); + } else if (msg->data == 4) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); + } else if (msg->data == 5) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); + } else if (msg->data == 6) { + command_buffer = servo_.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); + } + + servo_uart_data_ptr = command_buffer.get(); + transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); + + std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); + RCLCPP_INFO( + this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, + hex_string.c_str()); + } + + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) { + return "[]"; + } + + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + + for (size_t i = 0; i < size; ++i) { + ss << std::setw(2) << static_cast(data[i]) << " "; + } + + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') { + result.pop_back(); + } + return result; + } // DEBUG TOOL + + rclcpp::Logger logger_; + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(DartFillingTestHardware& robot) : dart_(robot) {} + void update() override { dart_.command_update(); } + DartFillingTestHardware& dart_; + }; + + std::shared_ptr dart_command_; + device::Dr16 dr16_; + device::TriggerServo trigger_servo_; + device::TriggerServo limiting_servo_; + device::TriggerServo lifting_left_; + device::TriggerServo lifting_right_; + rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; + rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; + rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; + rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + OutputInterface lifting_current_angle_left_; + OutputInterface lifting_current_angle_right_; + std::vector yaw_samples_, time_samples_; + uint16_t last_trigger_angle_ = 0xFFFF; + uint16_t last_limiting_angle_ = 0xFFFF; + uint16_t last_lifting_left_angle_ = 0xFFFF; + rclcpp::Time last_read_left_time_; + rclcpp::Time last_read_right_time_; + const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); +}; +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DartFillingTestHardware, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp new file mode 100644 index 00000000..c9fc7e7f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class HojoBoard { +public: + HojoBoard(rmcs_executor::Component& command_component, + const std::string& name, + uint8_t id_first, uint8_t id_second) + : id_first_(id_first), id_second_(id_second) { + command_component.register_input(name + "_first/control_angle", control_angle_first_); + command_component.register_input(name + "_second/control_angle", control_angle_second_); + command_component.register_input(name + "_first/runtime", runtime_first_); + command_component.register_input(name + "_second/runtime", runtime_second_); + + } + + std::unique_ptr generate_sync_run_command(size_t& output_length) { + size_t size = sizeof(WrightSyncControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightSyncControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0xFE; + command.data_length = 0x0E; + command.command_type = 0x83; + command.command_address = 0x2A; + command.command_address_length = 0x04; + command.cowork_id_one = id_first_; + command.control_angle_one[0] = static_cast(*control_angle_first_ >> 8); + command.control_angle_one[1] = static_cast(*control_angle_first_ & 0x00FF); + command.runtime_one[0] = static_cast(*runtime_first_ >> 8); + command.runtime_one[1] = static_cast(*runtime_first_ & 0x00FF); + command.cowork_id_two = id_second_; + command.control_angle_two[0] = static_cast(*control_angle_second_ >> 8); + command.control_angle_two[1] = static_cast(*control_angle_second_ & 0x00FF); + command.runtime_two[0] = static_cast(*runtime_second_ >> 8); + command.runtime_two[1] = static_cast(*runtime_second_ & 0x00FF); + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + return buffer; + } + +private: + struct __attribute__((packed)) WrightSyncControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t command_address_length; + uint8_t cowork_id_one; + uint8_t control_angle_one[2]; + uint8_t runtime_one[2]; + uint8_t cowork_id_two; + uint8_t control_angle_two[2]; + uint8_t runtime_two[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + command_address_length + + cowork_id_one + control_angle_one[0] + control_angle_one[1] + runtime_one[0] + runtime_one[1] + + cowork_id_two + control_angle_two[0] + control_angle_two[1] + runtime_two[0] + runtime_two[1]; + return ~check; + } + }; + + template + std::unique_ptr copy_to_buffer(const T& cmd, size_t& out_len) { + auto buffer = std::make_unique(sizeof(T)); + std::memcpy(buffer.get(), &cmd, sizeof(T)); + out_len = sizeof(T); + return buffer; + } + + uint8_t id_first_; + uint8_t id_second_; + + rmcs_executor::Component::InputInterface control_angle_first_; + rmcs_executor::Component::InputInterface control_angle_second_; + rmcs_executor::Component::InputInterface runtime_first_; + rmcs_executor::Component::InputInterface runtime_second_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp new file mode 100644 index 00000000..2fa9d356 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp @@ -0,0 +1,175 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class LiftingLeft { +public: + LiftingLeft(rmcs_executor::Component& command_component, const std::string& name) { + command_component.register_input(name + "/control_angle", control_angle_); + } + + enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, + }; + + bool calibrate_mode() const { return is_calibrate_mode_; } + + void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + + std::unique_ptr generate_runtime_command(size_t& output_length) { + size_t size = sizeof(WrightControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0x03; + command.data_length = 0x07; + command.command_type = 0x03; + command.command_address = 0x2A; + command.control_angle[0] = static_cast(*control_angle_ >> 8); + command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); + command.control_runtime[0] = 0x00; + command.control_runtime[1] = 0x00; + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + + return buffer; + } + + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + uint8_t* command = nullptr; + size_t command_size = 0; + + if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { + command_size = sizeof(switch_servo_mode_); + command = switch_servo_mode_; + } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { + command_size = sizeof(switch_motor_mode_); + command = switch_motor_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { + command_size = sizeof(switch_motor_forward_mode_); + command = switch_motor_forward_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { + command_size = sizeof(switch_motor_reverse_mode_); + command = switch_motor_reverse_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { + command_size = sizeof(motor_mode_runtime_control_); + command = motor_mode_runtime_control_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { + command_size = sizeof(motor_mode_disable_control_); + command = motor_mode_disable_control_; + } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { + command_size = sizeof(read_angle_command_); + command = read_angle_command_; + } + + std::unique_ptr buffer = std::make_unique(command_size); + std::memcpy(buffer.get(), command, command_size); + output_length = command_size; + + return buffer; + } + + bool calibrate_current_angle( + const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != sizeof(ReadAngleReceiveData)) { + RCLCPP_INFO(logger, "UART data length not match"); + return false; + } + + ReadAngleReceiveData package; + uint8_t checksum; + + std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); + std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + + if (package.header[0] != 0xFF || package.header[1] != 0xF5) { + RCLCPP_INFO(logger, "UART data header not match"); + return false; + } + + if (checksum != package.calculate_checksum()) { + RCLCPP_INFO( + logger, "Checksum error: receive:%x,calc:%x", checksum, + package.calculate_checksum()); + return false; + } + + uint16_t current_angle = static_cast(package.current_angle[0]) << 8 + | static_cast(package.current_angle[1]); + + current_angle_ = current_angle; + + RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); + return true; + } + + uint16_t get_current_angle() const { + return current_angle_.load(std::memory_order_acquire); + } + +private: + struct __attribute__((packed)) WrightControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t control_angle[2]; + uint8_t control_runtime[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + + control_angle[1] + control_runtime[0] + control_runtime[1]; + return ~check; + } + }; + + struct __attribute__((packed)) ReadAngleReceiveData { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t servo_status; + uint8_t current_angle[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; + return ~check; + } + }; + uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x02, 0x38, 0x02, 0xBC}; + + uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1C, 0x01, 0xD8}; + uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1C, 0x00, 0xD9}; + + uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1D, 0x00, 0xD8}; + uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1D, 0x01, 0xD7}; + + uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x03, 0x05, 0x03, 0x41, 0x00, 0x14, 0x9F}; + uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x03, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB3}; + rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + + std::atomic is_calibrate_mode_ = false; + std::atomic current_angle_{0}; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp new file mode 100644 index 00000000..f027f83c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp @@ -0,0 +1,176 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class LiftingRight { +public: + LiftingRight(rmcs_executor::Component& command_component, const std::string& name) { + command_component.register_input(name + "/control_angle", control_angle_); + } + + enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, + }; + + bool calibrate_mode() const { return is_calibrate_mode_; } + + void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + + std::unique_ptr generate_runtime_command(size_t& output_length) { + size_t size = sizeof(WrightControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0x04; + command.data_length = 0x07; + command.command_type = 0x03; + command.command_address = 0x2A; + command.control_angle[0] = static_cast(*control_angle_ >> 8); + command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); + command.control_runtime[0] = 0x00; + command.control_runtime[1] = 0x00; + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + + return buffer; + } + + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + uint8_t* command = nullptr; + size_t command_size = 0; + + if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { + command_size = sizeof(switch_servo_mode_); + command = switch_servo_mode_; + } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { + command_size = sizeof(switch_motor_mode_); + command = switch_motor_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { + command_size = sizeof(switch_motor_forward_mode_); + command = switch_motor_forward_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { + command_size = sizeof(switch_motor_reverse_mode_); + command = switch_motor_reverse_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { + command_size = sizeof(motor_mode_runtime_control_); + command = motor_mode_runtime_control_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { + command_size = sizeof(motor_mode_disable_control_); + command = motor_mode_disable_control_; + } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { + command_size = sizeof(read_angle_command_); + command = read_angle_command_; + } + + std::unique_ptr buffer = std::make_unique(command_size); + std::memcpy(buffer.get(), command, command_size); + output_length = command_size; + + return buffer; + } + + bool calibrate_current_angle( + const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != sizeof(ReadAngleReceiveData)) { + RCLCPP_INFO(logger, "UART data length not match"); + return false; + } + + ReadAngleReceiveData package; + uint8_t checksum; + + std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); + std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + + if (package.header[0] != 0xFF || package.header[1] != 0xF5) { + RCLCPP_INFO(logger, "UART data header not match"); + return false; + } + + if (checksum != package.calculate_checksum()) { + RCLCPP_INFO( + logger, "Checksum error: receive:%x,calc:%x", checksum, + package.calculate_checksum()); + return false; + } + + uint16_t current_angle = static_cast(package.current_angle[0]) << 8 + | static_cast(package.current_angle[1]); + + current_angle_.store(current_angle, std::memory_order_release); + + RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); + return true; + } + + uint16_t get_current_angle() const { + return current_angle_.load(std::memory_order_acquire); + } + +private: + struct __attribute__((packed)) WrightControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t control_angle[2]; + uint8_t control_runtime[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + + control_angle[1] + control_runtime[0] + control_runtime[1]; + return ~check; + } + }; + + struct __attribute__((packed)) ReadAngleReceiveData { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t servo_status; + uint8_t current_angle[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; + return ~check; + } + }; + uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x02, 0x38, 0x02, 0xBB}; + + uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1C, 0x01, 0xD7}; + uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1C, 0x00, 0xD8}; + + uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1D, 0x00, 0xD7}; + uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1D, 0x01, 0xD6}; + + uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x04, 0x05, 0x03, 0x41, 0x00, 0x14, 0x9E}; + uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x04, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB2}; + + rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + + std::atomic is_calibrate_mode_ = false; + std::atomic current_angle_{0}; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp new file mode 100644 index 00000000..6b8c4b64 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp @@ -0,0 +1,622 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class LimitingServo { +public: + LimitingServo(rmcs_executor::Component& command_component, const std::string& name) { + command_component.register_input(name + "/control_angle", control_angle_); + } + + enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, + }; + + bool calibrate_mode() const { return is_calibrate_mode_; } + + void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + + std::unique_ptr generate_runtime_command(size_t& output_length) { + size_t size = sizeof(WrightControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0x02; + command.data_length = 0x07; + command.command_type = 0x03; + command.command_address = 0x2A; + command.control_angle[0] = static_cast(*control_angle_ >> 8); + command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); + command.control_runtime[0] = 0x00; + command.control_runtime[1] = 0x00; + command.checksum = command.calculate_checksum(); + + std::memcpy(buffer.get(), &command, size); + output_length = size; + + return buffer; + } + + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + uint8_t* command = nullptr; + size_t command_size = 0; + + if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { + command_size = sizeof(switch_servo_mode_); + command = switch_servo_mode_; + } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { + command_size = sizeof(switch_motor_mode_); + command = switch_motor_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { + command_size = sizeof(switch_motor_forward_mode_); + command = switch_motor_forward_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { + command_size = sizeof(switch_motor_reverse_mode_); + command = switch_motor_reverse_mode_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { + command_size = sizeof(motor_mode_runtime_control_); + command = motor_mode_runtime_control_; + } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { + command_size = sizeof(motor_mode_disable_control_); + command = motor_mode_disable_control_; + } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { + command_size = sizeof(read_angle_command_); + command = read_angle_command_; + } + + std::unique_ptr buffer = std::make_unique(command_size); + std::memcpy(buffer.get(), command, command_size); + output_length = command_size; + + return buffer; + } + + static bool calibrate_current_angle( + const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != sizeof(ReadAngleReceiveData)) { + RCLCPP_INFO(logger, "UART data length not match"); + return false; + } + + ReadAngleReceiveData package; + uint8_t checksum; + + std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); + std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + + if (package.header[0] != 0xFF || package.header[1] != 0xF5) { + RCLCPP_INFO(logger, "UART data header not match"); + return false; + } + + if (checksum != package.calculate_checksum()) { + RCLCPP_INFO( + logger, "Checksum error: receive:%x,calc:%x", checksum, + package.calculate_checksum()); + return false; + } + + uint16_t current_angle = static_cast(package.current_angle[0]) << 8 + | static_cast(package.current_angle[1]); + RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); + return true; + } + +private: + struct __attribute__((packed)) WrightControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t control_angle[2]; + uint8_t control_runtime[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + + control_angle[1] + control_runtime[0] + control_runtime[1]; + return ~check; + } + }; + + struct __attribute__((packed)) ReadAngleReceiveData { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t servo_status; + uint8_t current_angle[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; + return ~check; + } + }; + uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x02, 0x38, 0x02, 0xBD}; + + uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1C, 0x01, 0xD9}; + uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1C, 0x00, 0xDA}; + + uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1D, 0x00, 0xD9}; + uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1D, 0x01, 0xD8}; + + uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x02, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA0}; + uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x02, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB4}; + + rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + + std::atomic is_calibrate_mode_ = false; +}; +} + +// namespace rmcs_core::hardware::device +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware::device { + +// enum class CalibrateOperation { +// SWITCH_TO_SERVO_MODE, +// SWITCH_TO_MOTOR_MODE, +// MOTOR_FORWARD_MODE, +// MOTOR_REVERSE_MODE, +// MOTOR_RUNTIME_CONTROL, +// MOTOR_DISABLE_CONTROL, +// READ_CURRENT_ANGLE, +// }; + +// class TriggerServo { +// public: +// TriggerServo(rmcs_executor::Component& command_component, const std::string& name, uint8_t device_id_ = 0x01) { +// command_component.register_input(name + "/control_angle", control_angle_, device_id_); +// } +// bool calibrate_mode() const { return is_calibrate_mode_; } + +// void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + +// std::unique_ptr generate_runtime_command(size_t& output_length) { +// size_t size = sizeof(WrightControlAngleCommand); +// std::unique_ptr buffer = std::make_unique(size); +// WrightControlAngleCommand command; +// command.header[0] = 0xFF; +// command.header[1] = 0xFF; +// command.id = device_id_; +// command.data_length = 0x07; +// command.command_type = 0x03; +// command.command_address = 0x2A; +// command.control_angle[0] = static_cast(*control_angle_ >> 8); +// command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); +// command.control_runtime[0] = 0x00; +// command.control_runtime[1] = 0x00; +// command.checksum = command.calculate_checksum(); + +// std::memcpy(buffer.get(), &command, size); +// output_length = size; + +// // RCLCPP_INFO( +// // logger, "%x, %x, %x, %x", *control_angle_, *control_angle_ >> 8, static_cast(*control_angle_ >> +// // 8), static_cast(*control_angle_ & 0x00FF)); +// return buffer; +// } + +// std::unique_ptr +// generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { +// uint8_t* command = nullptr; +// size_t command_size = 0; + +// if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { +// command_size = sizeof(switch_servo_mode_); +// command = switch_servo_mode_; +// } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { +// command_size = sizeof(switch_motor_mode_); +// command = switch_motor_mode_; +// } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { +// command_size = sizeof(switch_motor_forward_mode_); +// command = switch_motor_forward_mode_; +// } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { +// command_size = sizeof(switch_motor_reverse_mode_); +// command = switch_motor_reverse_mode_; +// } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { +// command_size = sizeof(motor_mode_runtime_control_); +// command = motor_mode_runtime_control_; +// } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { +// command_size = sizeof(motor_mode_disable_control_); +// command = motor_mode_disable_control_; +// } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { +// command_size = sizeof(read_angle_command_); +// command = read_angle_command_; +// } + +// std::unique_ptr buffer = std::make_unique(command_size); +// std::memcpy(buffer.get(), command, command_size); +// output_length = command_size; + +// return buffer; +// } + +// bool +// calibrate_current_angle(const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { +// if (uart_data_length != sizeof(ReadAngleReceiveData)) { +// RCLCPP_INFO(logger, "UART data length not match"); +// return false; +// } + +// ReadAngleReceiveData package; +// uint8_t checksum; + +// std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); +// std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); + +// if (package.header[0] != 0xFF || package.header[1] != 0xF5) { +// RCLCPP_INFO(logger, "UART data header not match"); +// return false; +// } + +// if (checksum != package.calculate_checksum()) { +// RCLCPP_INFO(logger, "Checksum error: receive:%x,calc:%x", checksum, package.calculate_checksum()); +// return false; +// } + +// current_angle_ = +// static_cast(package.current_angle[0]) << 8 | static_cast(package.current_angle[1]); +// RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle_); +// return true; +// } + +// uint16_t get_current_angle_() const { return current_angle_; } + +// private: +// uint8_t device_id_; +// uint16_t current_angle_; + +// struct __attribute__((packed)) WrightControlAngleCommand { +// uint8_t header[2]; +// uint8_t id; +// uint8_t data_length; +// uint8_t command_type; +// uint8_t command_address; +// uint8_t control_angle[2]; +// uint8_t control_runtime[2]; +// uint8_t checksum; + +// uint8_t calculate_checksum() const { +// uint8_t check = id + data_length + command_type + command_address + control_angle[0] + control_angle[1] +// + control_runtime[0] + control_runtime[1]; +// return ~check; +// } +// }; + +// struct __attribute__((packed)) ReadAngleReceiveData { +// uint8_t header[2]; +// uint8_t id; +// uint8_t data_length; +// uint8_t servo_status; +// uint8_t current_angle[2]; +// uint8_t checksum; + +// uint8_t calculate_checksum() const { +// uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; +// return ~check; +// } +// }; +// uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02, 0xBE}; + +// uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x01, 0xDA}; +// uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x00, 0xDB}; + +// uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x00, 0xDA}; +// uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x01, 0xD9}; + +// uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA1}; +// uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB5}; + +// rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + +// std::atomic is_calibrate_mode_ = false; +// }; +// } +// namespace rmcs_core::hardware::device +// // #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/force_sensor.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include "librmcs/client/cboard.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware { +// class CatapultDart +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// CatapultDart() +// : 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()) +// , dart_command_( +// create_partner_component(get_component_name() + "_command", *this)) +// , dr16_(*this) +// , pitch_motor_( +// *this, *dart_command_, "/dart/pitch_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , yaw_motor_( +// *this, *dart_command_, "/dart/yaw_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , force_control_motor_( +// *this, *dart_command_, "/dart/force_screw_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , drive_belt_motor_( +// {*this, *dart_command_, "/dart/drive_belt/left", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, +// {*this, *dart_command_, "/dart/drive_belt/right", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(19.) +// .set_reversed()}) +// , force_sensor_(*this) +// , trigger_servo_(*dart_command_, "/dart/trigger_servo") +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// force_sensor_calibrate_subscription_ = create_subscription( +// "/force_sensor/calibrate", rclcpp::QoS{0}, +// [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// force_sensor_calibrate_subscription_callback(std::move(msg)); +// }); + +// trigger_calibrate_subscription_ = create_subscription( +// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_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; +// }; +// } + +// ~CatapultDart() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// dr16_.update_status(); +// pitch_motor_.update_status(); +// yaw_motor_.update_status(); +// drive_belt_motor_[0].update_status(); +// drive_belt_motor_[1].update_status(); +// force_control_motor_.update_status(); +// force_sensor_.update_status(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// if (force_sensor_pub_time_count_++ > 100) { +// force_sensor_pub_time_count_ = 0; + +// transmit_buffer_.add_can1_transmission( +// force_sensor_.get_can_id(device::ForceSensor::Mode::MEASUREMENT, 1), +// force_sensor_.generate_read_weight_command()); +// } + +// can_commands[0] = pitch_motor_.generate_command(); +// can_commands[1] = yaw_motor_.generate_command(); +// can_commands[2] = force_control_motor_.generate_command(); +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// can_commands[0] = drive_belt_motor_[0].generate_command(); +// can_commands[1] = drive_belt_motor_[1].generate_command(); +// can_commands[2] = 0; +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + +// if (!trigger_servo_.calibrate_mode()) { +// size_t uart_data_length; +// std::unique_ptr command_buffer = +// trigger_servo_.generate_runtime_command(uart_data_length); +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); +// } + +// transmit_buffer_.trigger_transmission(); +// } + +// 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 == 0x302) { +// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x201) { +// pitch_motor_.store_status(can_data); +// } else if (can_id == 0x202) { +// yaw_motor_.store_status(can_data); +// } else if (can_id == 0x203) { +// force_control_motor_.store_status(can_data); +// } else if (can_id == 0x205) { +// drive_belt_motor_[0].store_status(can_data); +// } else if (can_id == 0x206) { +// drive_belt_motor_[1].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 uart2_receive_callback(const std::byte* data, uint8_t length) override { +// // bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); +// // if (!success) { +// // RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// // } + +// // // std::string hex_string = bytes_to_hex_string(data, length); +// // // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, +// // // hex_string.c_str()); +// // } + +// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { +// dr16_.store_status(uart_data, uart_data_length); +// } + +// private: +// void force_sensor_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// transmit_buffer_.add_can1_transmission( +// force_sensor_.get_can_id(device::ForceSensor::Mode::ZERO_CALIBRATE, 1), +// force_sensor_.generate_zero_calibration_command()); +// RCLCPP_INFO(logger_, "call"); +// } + +// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { +// /* +// 标定命令格式： +// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" +// 替换data值就行 +// */ +// trigger_servo_.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; +// if (msg->data == 0) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = trigger_servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); + +// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); +// RCLCPP_INFO( +// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, +// hex_string.c_str()); +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) { +// return "[]"; +// } + +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); + +// for (size_t i = 0; i < size; ++i) { +// ss << std::setw(2) << static_cast(data[i]) << " "; +// } + +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') { +// result.pop_back(); +// } +// return result; +// } // DEBUG TOOL + +// rclcpp::Logger logger_; + +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDart& robot) +// : dart_(robot) {} + +// void update() override { dart_.command_update(); } + +// CatapultDart& dart_; +// }; +// std::shared_ptr dart_command_; + +// device::Dr16 dr16_; + +// device::DjiMotor pitch_motor_; +// device::DjiMotor yaw_motor_; +// device::DjiMotor force_control_motor_; +// device::DjiMotor drive_belt_motor_[2]; + +// device::ForceSensor force_sensor_; +// int force_sensor_pub_time_count_ = 0; // ForceSensor仅支持10Hz + +// device::TriggerServo trigger_servo_; + +// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; + +// 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::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp index bf695f24..d5611ed7 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp @@ -1,32 +1,40 @@ +#pragma once + #include #include #include #include +#include #include #include #include #include namespace rmcs_core::hardware::device { - -enum class CalibrateOperation { - SWITCH_TO_SERVO_MODE, - SWITCH_TO_MOTOR_MODE, - MOTOR_FORWARD_MODE, - MOTOR_REVERSE_MODE, - MOTOR_RUNTIME_CONTROL, - MOTOR_DISABLE_CONTROL, - READ_CURRENT_ANGLE, -}; - class TriggerServo { public: - TriggerServo(rmcs_executor::Component& command_component, const std::string& name) { + TriggerServo(rmcs_executor::Component& command_component, + const std::string& name, + uint8_t id) + : id_(id) { command_component.register_input(name + "/control_angle", control_angle_); } - bool calibrate_mode() const { return is_calibrate_mode_; } + enum class CalibrateOperation { + SWITCH_TO_SERVO_MODE, + SWITCH_TO_MOTOR_MODE, + MOTOR_FORWARD_MODE, + MOTOR_REVERSE_MODE, + MOTOR_RUNTIME_CONTROL, + MOTOR_DISABLE_CONTROL, + READ_CURRENT_ANGLE, + }; + + bool calibrate_mode() const { return is_calibrate_mode_; } void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } + uint16_t get_target_angle() const { + return *control_angle_; + } std::unique_ptr generate_runtime_command(size_t& output_length) { size_t size = sizeof(WrightControlAngleCommand); @@ -34,7 +42,7 @@ class TriggerServo { WrightControlAngleCommand command; command.header[0] = 0xFF; command.header[1] = 0xFF; - command.id = 0x01; + command.id = id_; command.data_length = 0x07; command.command_type = 0x03; command.command_address = 0x2A; @@ -46,53 +54,96 @@ class TriggerServo { std::memcpy(buffer.get(), &command, size); output_length = size; - - // RCLCPP_INFO( - // logger, "%x, %x, %x, %x", *control_angle_, *control_angle_ >> 8, static_cast(*control_angle_ >> - // 8), static_cast(*control_angle_ & 0x00FF)); return buffer; } - std::unique_ptr - generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { - uint8_t* command = nullptr; - size_t command_size = 0; - - if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { - command_size = sizeof(switch_servo_mode_); - command = switch_servo_mode_; - } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { - command_size = sizeof(switch_motor_mode_); - command = switch_motor_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { - command_size = sizeof(switch_motor_forward_mode_); - command = switch_motor_forward_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { - command_size = sizeof(switch_motor_reverse_mode_); - command = switch_motor_reverse_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { - command_size = sizeof(motor_mode_runtime_control_); - command = motor_mode_runtime_control_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { - command_size = sizeof(motor_mode_disable_control_); - command = motor_mode_disable_control_; - } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { - command_size = sizeof(read_angle_command_); - command = read_angle_command_; - } - - std::unique_ptr buffer = std::make_unique(command_size); - std::memcpy(buffer.get(), command, command_size); - output_length = command_size; + static std::unique_ptr generate_sync_run_command(size_t& output_length + , uint8_t cowork_id_one, uint8_t cowork_id_two + , uint16_t control_angle_one, uint16_t control_angle_two + , uint16_t runtime_one, uint16_t runtime_two) { + size_t size = sizeof(WrightSyncControlAngleCommand); + std::unique_ptr buffer = std::make_unique(size); + WrightSyncControlAngleCommand command; + command.header[0] = 0xFF; + command.header[1] = 0xFF; + command.id = 0xFE; + command.data_length = 0x0E; + command.command_type = 0x83; + command.command_address = 0x2A; + command.command_address_length = 0x04; + command.cowork_id_one = cowork_id_one; + command.control_angle_one[0] = static_cast(control_angle_one >> 8); + command.control_angle_one[1] = static_cast(control_angle_one & 0x00FF); + command.runtime_one[0] = static_cast(runtime_one >> 8); + command.runtime_one[1] = static_cast(runtime_one & 0x00FF); + command.cowork_id_two = cowork_id_two; + command.control_angle_two[0] = static_cast(control_angle_two >> 8); + command.control_angle_two[1] = static_cast(control_angle_two & 0x00FF); + command.runtime_two[0] = static_cast(runtime_two >> 8); + command.runtime_two[1] = static_cast(runtime_two & 0x00FF); + command.checksum = command.calculate_checksum(); + std::memcpy(buffer.get(), &command, size); + output_length = size; return buffer; } - static bool - calibrate_current_angle(const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { + std::unique_ptr + generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { + switch (calibrate_operation) { + case CalibrateOperation::SWITCH_TO_SERVO_MODE: { + SwitchServoModeCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::SWITCH_TO_MOTOR_MODE: { + SwitchMotorModeCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::MOTOR_FORWARD_MODE: { + MotorForwardModeCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::MOTOR_REVERSE_MODE: { + MotorReverseModeCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::MOTOR_RUNTIME_CONTROL: { + MotorRuntimeControlCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::MOTOR_DISABLE_CONTROL: { + MotorDisableControlCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + case CalibrateOperation::READ_CURRENT_ANGLE: { + ReadAngleCommand command; + command.id = id_; + command.checksum = command.calculate_checksum(); + return copy_to_buffer(command, output_length); + } + default: + output_length = 0; + return nullptr; + } + } + + std::pair calibrate_current_angle ( + const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { if (uart_data_length != sizeof(ReadAngleReceiveData)) { RCLCPP_INFO(logger, "UART data length not match"); - return false; + return {false, 0x0000}; } ReadAngleReceiveData package; @@ -103,18 +154,68 @@ class TriggerServo { if (package.header[0] != 0xFF || package.header[1] != 0xF5) { RCLCPP_INFO(logger, "UART data header not match"); - return false; + return {false, 0x0000}; } + if (package.id != id_) { + RCLCPP_INFO(logger, "id not match"); + return {false, 0x0000}; + } if (checksum != package.calculate_checksum()) { - RCLCPP_INFO(logger, "Checksum error: receive:%x,calc:%x", checksum, package.calculate_checksum()); - return false; + RCLCPP_INFO(logger, "Checksum error: receive:%x,calc:%x", + checksum, package.calculate_checksum()); + return {false, 0x0000}; } - uint16_t current_angle = - static_cast(package.current_angle[0]) << 8 | static_cast(package.current_angle[1]); - RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); - return true; + uint16_t current_angle = static_cast(package.current_angle[0]) << 8 + | static_cast(package.current_angle[1]); + current_angle_.store(current_angle, std::memory_order_release); + + RCLCPP_INFO(logger, "%d Current Angle: 0x%04X", id_, current_angle); + return {true, current_angle}; + } + + std::pair fixable_calibrate_current_angle( + const rclcpp::Logger& logger, + const std::byte* uart_data, + size_t uart_data_length, + uint16_t& out_angle) { + if (uart_data_length < 7) { + RCLCPP_WARN(logger, "UART data too short: %zu bytes (need at least 7)", uart_data_length); + return {false, 0x0000}; + } + + if (static_cast(uart_data[0]) != 0xFF || + static_cast(uart_data[1]) != 0xF5) { + RCLCPP_DEBUG(logger, "UART header mismatch: %02X %02X", + static_cast(uart_data[0]), static_cast(uart_data[1])); + return {false, 0x0000}; + } + + uint8_t id = static_cast(uart_data[2]); + + if (uart_data_length >= 8) { + uint8_t data_length = static_cast(uart_data[3]); + uint8_t servo_status = static_cast(uart_data[4]); + uint8_t angle_high = static_cast(uart_data[5]); + uint8_t angle_low = static_cast(uart_data[6]); + uint8_t received_checksum = static_cast(uart_data[7]); + + uint8_t calculated_checksum = ~(id + data_length + servo_status + angle_high + angle_low); + if (received_checksum != calculated_checksum) { + RCLCPP_WARN(logger, "Checksum error: received 0x%02X, calculated 0x%02X", + received_checksum, calculated_checksum); + } + } + + uint16_t angle = static_cast(static_cast(uart_data[5])) << 8 + | static_cast(static_cast(uart_data[6])); + out_angle = angle; + + current_angle_.store(angle, std::memory_order_release); + + RCLCPP_DEBUG(logger, "Servo ID 0x%02X current angle: 0x%04X", id, angle); + return {true, angle}; } private: @@ -129,12 +230,132 @@ class TriggerServo { uint8_t checksum; uint8_t calculate_checksum() const { - uint8_t check = id + data_length + command_type + command_address + control_angle[0] + control_angle[1] - + control_runtime[0] + control_runtime[1]; + uint8_t check = id + data_length + command_type + command_address + control_angle[0] + + control_angle[1] + control_runtime[0] + control_runtime[1]; return ~check; } }; + struct __attribute__((packed)) WrightSyncControlAngleCommand { + uint8_t header[2]; + uint8_t id; + uint8_t data_length; + uint8_t command_type; + uint8_t command_address; + uint8_t command_address_length; + uint8_t cowork_id_one; + uint8_t control_angle_one[2]; + uint8_t runtime_one[2]; + uint8_t cowork_id_two; + uint8_t control_angle_two[2]; + uint8_t runtime_two[2]; + uint8_t checksum; + + uint8_t calculate_checksum() const { + uint8_t check = id + data_length + command_type + command_address + command_address_length + + cowork_id_one + control_angle_one[0] + control_angle_one[1] + runtime_one[0] + runtime_one[1] + + cowork_id_two + control_angle_two[0] + control_angle_two[1] + runtime_two[0] + runtime_two[1]; + return ~check; + } + }; + struct __attribute__((packed)) SwitchServoModeCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x04; + uint8_t command_type = 0x03; + uint8_t command_address = 0x1C; + uint8_t data = 0x01; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data); + } + }; + + struct __attribute__((packed)) SwitchMotorModeCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x04; + uint8_t command_type = 0x03; + uint8_t command_address = 0x1C; + uint8_t data = 0x00; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data); + } + }; + + struct __attribute__((packed)) MotorForwardModeCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x04; + uint8_t command_type = 0x03; + uint8_t command_address = 0x1D; + uint8_t data = 0x00; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data); + } + }; + + struct __attribute__((packed)) MotorReverseModeCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x04; + uint8_t command_type = 0x03; + uint8_t command_address = 0x1D; + uint8_t data = 0x01; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data); + } + }; + + struct __attribute__((packed)) MotorRuntimeControlCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x05; + uint8_t command_type = 0x03; + uint8_t command_address = 0x41; + uint8_t data[2] = {0x00, 0x14}; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data[0] + data[1]); + } + }; + + struct __attribute__((packed)) MotorDisableControlCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x05; + uint8_t command_type = 0x03; + uint8_t command_address = 0x41; + uint8_t data[2] = {0x00, 0x00}; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data[0] + data[1]); + } + }; + + struct __attribute__((packed)) ReadAngleCommand { + uint8_t header[2] = {0xFF, 0xFF}; + uint8_t id; + uint8_t data_length = 0x04; + uint8_t command_type = 0x02; + uint8_t command_address = 0x38; + uint8_t data = 0x02; + uint8_t checksum; + + uint8_t calculate_checksum() const { + return ~(id + data_length + command_type + command_address + data); + } + }; + struct __attribute__((packed)) ReadAngleReceiveData { uint8_t header[2]; uint8_t id; @@ -148,19 +369,19 @@ class TriggerServo { return ~check; } }; - uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02, 0xBE}; - uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x01, 0xDA}; - uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x00, 0xDB}; - - uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x00, 0xDA}; - uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x01, 0xD9}; - - uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA1}; - uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB5}; - - rmcs_executor::Component::InputInterface control_angle_; // 0-4095 + template + std::unique_ptr copy_to_buffer(const T& cmd, size_t& out_len) { + auto buffer = std::make_unique(sizeof(T)); + std::memcpy(buffer.get(), &cmd, sizeof(T)); + out_len = sizeof(T); + return buffer; + } + rmcs_executor::Component::InputInterface control_angle_; std::atomic is_calibrate_mode_ = false; + std::atomic current_angle_{0}; + uint8_t id_; }; + } // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp deleted file mode 100644 index fdf11e56..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/test3.cpp +++ /dev/null @@ -1,589 +0,0 @@ -// #include "librmcs/client/cboard.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/bmi088.hpp" -// #include "hardware/device/force_sensor_runtime.hpp" -// #include "hardware/device/trigger_servo.hpp" -// #include -// #include -// #include -// #include -// #include -// #include "filter/low_pass_filter.hpp" -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware { - -// class CatapultDart -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::client::CBoard { -// public: -// CatapultDart() -// : 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()) -// , pitch_angle_filter_(20.0, 1000.0) -// , pitch_velocity_filter_(20.0, 1000.0) -// , yaw_velocity_filter_(20.0, 1000.0) -// , yaw_filter_(5.0, 1000.0) -// , pitch_filter_(10.0, 1000.0) -// , roll_filter_(10.0, 1000.0) -// , dart_command_(create_partner_component(get_component_name() + "_command", *this)) -// , dr16_(*this) -// , imu_(1000, 0.2, 0.0) -// , pitch_motor_( -// *this, *dart_command_, "/dart/pitch_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , yaw_motor_( -// *this, *dart_command_, "/dart/yaw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , force_control_motor_( -// *this, *dart_command_, "/dart/force_control_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , drive_belt_motor_( -// {*this, *dart_command_, "/dart/drive_belt/left", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, -// {*this, *dart_command_, "/dart/drive_belt/right", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508} -// .set_reduction_ratio(19.) -// .set_reversed()}) -// , force_sensor_(*this) -// , trigger_servo_(*dart_command_, "/dart/trigger_servo") -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { - -// // Register outputs for TF and IMU state -// register_output("/dart/pitch/angle", pitch_angle_); -// register_output("/tf", tf_); -// register_output("/imu/state/final_roll", final_roll); -// register_output("/imu/state/final_pitch", final_pitch); -// register_output("/imu/state/final_yaw", final_yaw); - -// // Debug outputs for raw angles from IMU (no transformation) -// register_output("/imu/debug/raw_roll", debug_raw_roll_); -// register_output("/imu/debug/raw_pitch", debug_raw_pitch_); -// register_output("/imu/debug/raw_yaw", debug_raw_yaw_); - -// // Debug outputs for transformed angles (unnormalized and normalized) -// register_output("/imu/debug/transformed_roll_unnorm", debug_transformed_roll_unnorm_); -// register_output("/imu/debug/transformed_pitch_unnorm", debug_transformed_pitch_unnorm_); -// register_output("/imu/debug/transformed_yaw_unnorm", debug_transformed_yaw_unnorm_); - -// register_output("/imu/debug/transformed_roll_norm", debug_transformed_roll_norm_); -// register_output("/imu/debug/transformed_pitch_norm", debug_transformed_pitch_norm_); -// register_output("/imu/debug/transformed_yaw_norm", debug_transformed_yaw_norm_); - -// // Additional debug outputs for drift compensation -// register_output("/imu/debug/drift_coefficient", debug_drift_coefficient_); -// register_output("/imu/debug/calibration_complete", debug_calibration_complete_); -// register_output("/imu/debug/compensation_applied", debug_compensation_applied_); - -// // Get IMU parameters -// imu_sensitivity_ = this->get_parameter("imu_sensitivity").as_double(); -// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); -// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - -// // Get initial offset parameters (default to 0.0) -// this->declare_parameter("roll_offset", 0.0); -// this->declare_parameter("pitch_offset", 0.0); -// this->declare_parameter("yaw_offset", 0.0); - -// roll_offset_ = this->get_parameter("roll_offset").as_double(); -// pitch_offset_ = this->get_parameter("pitch_offset").as_double(); -// yaw_offset_ = this->get_parameter("yaw_offset").as_double(); - -// setup_imu_coordinate_mapping(); -// imu_sampler_initialize(); -// tf_broadcaster_ = std::make_unique(*this); -// start_time_ = std::chrono::steady_clock::now(); - -// // Create subscription for trigger calibration -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback(std::move(msg)); -// }); -// } - -// ~CatapultDart() override { -// stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); - -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_[0].update_status(); -// drive_belt_motor_[1].update_status(); -// force_control_motor_.update_status(); -// force_sensor_.update_status(); - -// imu_.update_status(); -// processImuData(); -// } - -// void command_update() { -// uint16_t can_commands[4]; - -// if (pub_time_count_++ > 100) { -// transmit_buffer_.add_can1_transmission( -// 0x301, std::bit_cast(force_sensor_.generate_command())); -// pub_time_count_ = 0; -// } - -// can_commands[0] = pitch_motor_.generate_command(); -// can_commands[1] = yaw_motor_.generate_command(); -// can_commands[2] = force_control_motor_.generate_command(); -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - -// can_commands[0] = drive_belt_motor_[0].generate_command(); -// can_commands[1] = drive_belt_motor_[1].generate_command(); -// can_commands[2] = 0; -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - -// if (!trigger_servo_.calibrate_mode()) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); -// } - -// transmit_buffer_.trigger_transmission(); -// } -// int pub_time_count_ = 0; - -// 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 == 0x302) { -// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] -// return; - -// if (can_id == 0x201) { -// pitch_motor_.store_status(can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(can_data); -// } else if (can_id == 0x203) { -// force_control_motor_.store_status(can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_[0].store_status(can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_[1].store_status(can_data); -// } -// } - -// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_accelerometer_status(x, y, z); -// } - -// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_gyroscope_status(x, y, z); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!success) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// } - -// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { -// dr16_.store_status(uart_data, uart_data_length); -// } - -// private: -// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { -// trigger_servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } - -// void setup_imu_coordinate_mapping() { -// // Setup coordinate mapping for IMU raw data -// imu_.set_coordinate_mapping( -// [](double x, double y, double z) -> std::tuple { -// return {x, -y, -z}; -// }); -// } - -// void imu_sampler_initialize() { -// // Initialize IMU sampler variables -// start_time_ = std::chrono::steady_clock::now(); -// calibration_started_ = false; -// calibration_complete_ = false; -// yaw_drift_coefficient_ = 0.0; -// yaw_at_first_sample_ = 0.0; -// yaw_at_final_sample_ = 0.0; - -// RCLCPP_INFO(logger_, "IMU sampler initialized"); -// } - -// void processImuData() { -// auto current_time = std::chrono::steady_clock::now(); -// double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); - -// if (!calibration_started_) { -// calibration_started_ = true; -// RCLCPP_INFO(logger_, "IMU calibration started at time: %f seconds", elapsed_seconds); -// } - -// // Get quaternion from IMU -// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - -// // Convert quaternion to rotation matrix -// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - -// // Extract Euler angles (roll, pitch, yaw) from rotation matrix -// // Using convention: roll (X), pitch (Y), yaw (Z) -// double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); -// double pitch = std::asin(-rotation_matrix(2,0)); -// double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); - -// // Output raw angles without any transformation -// *debug_raw_roll_ = roll; -// *debug_raw_pitch_ = pitch; -// *debug_raw_yaw_ = yaw; - -// // Apply transformations for inverted IMU mounting: -// // 1. 给俯仰角（pitch）补偿π -// // 2. 俯仰和偏航的增量取反（乘以-1） -// // 3. Roll轴保持不变（根据您的描述，roll可能不需要特殊处理） -// double transformed_roll = -roll ; -// double transformed_pitch = pitch ; // 增量取反并补偿π -// double transformed_yaw = -yaw; // 增量取反 - -// // Apply initial offsets (if any) -// transformed_roll -= roll_offset_; -// transformed_pitch -= pitch_offset_; -// transformed_yaw -= yaw_offset_; - -// // Output unnormalized transformed angles -// *debug_transformed_roll_unnorm_ = (transformed_roll / M_PI) * 180; -// *debug_transformed_pitch_unnorm_ = (transformed_pitch / M_PI) * 180; -// *debug_transformed_yaw_unnorm_ = (transformed_yaw /M_PI) * 180; - -// // Normalize angles to [-π, π] -// transformed_roll = normalizeAngle(transformed_roll); -// transformed_pitch = normalizeAngle(transformed_pitch); -// transformed_yaw = normalizeAngle(transformed_yaw); - -// // Output normalized transformed angles -// *debug_transformed_roll_norm_ = transformed_roll; -// *debug_transformed_pitch_norm_ = transformed_pitch; -// *debug_transformed_yaw_norm_ = transformed_yaw; - -// // Apply low-pass filtering to normalized angles -// double filtered_roll = roll_filter_.update(transformed_roll); -// double filtered_pitch = pitch_filter_.update(transformed_pitch); -// double filtered_yaw = yaw_filter_.update(transformed_yaw); - -// // Yaw drift calibration using two-point method (use filtered yaw) -// if (!calibration_complete_) { -// // First sample point -// if (elapsed_seconds >= first_sample_spot_ && -// std::abs(yaw_at_first_sample_) < 0.001) { // Use tolerance for comparison - -// yaw_at_first_sample_ = filtered_yaw; -// first_sample_time_ = elapsed_seconds; -// RCLCPP_INFO(logger_, "First yaw sample: %f rad at time: %f s", -// yaw_at_first_sample_, first_sample_time_); -// } - -// // Final sample point -// if (elapsed_seconds >= final_sample_spot_ && -// std::abs(yaw_at_final_sample_) < 0.001) { - -// yaw_at_final_sample_ = filtered_yaw; -// final_sample_time_ = elapsed_seconds; - -// // Calculate drift coefficient (radians per second) -// if (std::abs(yaw_at_first_sample_) > 0.001) { // Ensure first sample is recorded -// double time_diff = final_sample_time_ - first_sample_time_; -// double yaw_diff = yaw_at_final_sample_ - yaw_at_first_sample_; - -// if (time_diff > 0.1) { // Ensure sufficient time difference -// yaw_drift_coefficient_ = yaw_diff / time_diff; -// calibration_complete_ = true; - -// RCLCPP_INFO(logger_, "Yaw calibration complete"); -// RCLCPP_INFO(logger_, " First sample: %f rad at %f s", -// yaw_at_first_sample_, first_sample_time_); -// RCLCPP_INFO(logger_, " Final sample: %f rad at %f s", -// yaw_at_final_sample_, final_sample_time_); -// RCLCPP_INFO(logger_, " Time difference: %f s", time_diff); -// RCLCPP_INFO(logger_, " Yaw difference: %f rad", yaw_diff); -// RCLCPP_INFO(logger_, " Drift coefficient: %f rad/s", -// yaw_drift_coefficient_); -// } -// } -// } -// } - -// // Apply yaw drift compensation (use filtered yaw) -// double compensated_yaw = filtered_yaw; -// bool compensation_applied = false; - -// if (calibration_complete_) { -// // Calculate time since calibration start -// double time_since_calibration = elapsed_seconds - first_sample_time_; - -// // Apply linear drift compensation to filtered yaw -// compensated_yaw = filtered_yaw - (yaw_drift_coefficient_ * time_since_calibration); -// compensation_applied = true; - -// // Normalize compensated yaw -// compensated_yaw = normalizeAngle(compensated_yaw); -// } - -// // Output final normalized angles (use filtered roll/pitch and compensated yaw) -// *final_roll = filtered_roll; -// *final_pitch = filtered_pitch; -// *final_yaw = compensated_yaw; - -// // Output debug information -// *debug_drift_coefficient_ = yaw_drift_coefficient_; -// *debug_calibration_complete_ = calibration_complete_ ? 1.0 : 0.0; -// *debug_compensation_applied_ = compensation_applied ? 1.0 : 0.0; - -// // Publish TF transforms using filtered roll/pitch and compensated yaw -// // 注意：需要将转换应用到odom坐标系 -// publishTfTransforms(filtered_roll, filtered_pitch, compensated_yaw); -// } - -// static double normalizeAngle(double angle) { -// // Normalize angle to range [-π, π] -// while (angle > M_PI) angle -= 2.0 * M_PI; -// while (angle < -M_PI) angle += 2.0 * M_PI; -// return angle; -// } - -// void publishTfTransforms(double roll, double pitch, double yaw) { -// auto now = this->get_clock()->now(); - -// // Helper function to create transform -// auto create_transform = [&](const std::string& parent, const std::string& child, -// const geometry_msgs::msg::Vector3& trans, -// const geometry_msgs::msg::Quaternion& rot) { -// geometry_msgs::msg::TransformStamped tf; -// tf.header.stamp = now; -// tf.header.frame_id = parent; -// tf.child_frame_id = child; -// tf.transform.translation = trans; -// tf.transform.rotation = rot; -// return tf; -// }; - -// // Helper function to create translation -// auto create_translation = [](double x, double y, double z) { -// geometry_msgs::msg::Vector3 t; -// t.x = x; t.y = y; t.z = z; -// return t; -// }; - -// // Helper function to create rotation -// auto create_rotation = [](double x, double y, double z, double w) { -// geometry_msgs::msg::Quaternion r; -// r.x = x; r.y = y; r.z = z; r.w = w; -// return r; -// }; - -// // Create zero translation -// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); -// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - -// // Create quaternions for each axis rotation -// // 注意：这里使用的roll, pitch, yaw已经是经过转换、滤波和补偿的最终值 -// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); -// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); -// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - -// // Combined rotation: yaw -> pitch -> roll -// // 这个组合顺序需要根据您的TF树结构来确定 -// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - -// // Publish all TF transforms -// // 注意：将最终转换应用到odom_imu坐标系 -// tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", -// zero_trans, create_rotation(0, 0, 0, 1))); - -// // Yaw only rotation -// tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", -// zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), -// yaw_quat.z(), yaw_quat.w()))); - -// // Pitch only rotation (注意：这里使用了转换后的pitch值) -// tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", -// pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), -// pitch_quat.z(), pitch_quat.w()))); - -// // Combined rotation for odom_imu frame (应用了所有转换) -// tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", -// zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), -// combined_quaternion.z(), combined_quaternion.w()))); - -// // World to base_link transform -// tf_broadcaster_->sendTransform(create_transform("world", "base_link", -// zero_trans, create_rotation(0, 0, 0, 1))); -// } - -// rclcpp::Logger logger_; - -// // Low-pass filters for IMU data -// filter::LowPassFilter<1> pitch_angle_filter_; -// filter::LowPassFilter<1> pitch_velocity_filter_; -// filter::LowPassFilter<1> yaw_velocity_filter_; -// filter::LowPassFilter<1> yaw_filter_; -// filter::LowPassFilter<1> pitch_filter_; -// filter::LowPassFilter<1> roll_filter_; - -// std::chrono::steady_clock::time_point start_time_; - -// // Dart command component -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDart& robot) -// : dart_(robot) {} - -// void update() override { dart_.command_update(); } - -// CatapultDart& dart_; -// }; -// std::shared_ptr dart_command_; - -// // IMU parameters -// double imu_sensitivity_; -// double first_sample_spot_; -// double final_sample_spot_; -// double roll_offset_; -// double pitch_offset_; -// double yaw_offset_; - -// // TF related -// OutputInterface tf_; -// std::unique_ptr tf_broadcaster_; - -// // Hardware devices -// device::Dr16 dr16_; -// device::Bmi088 imu_; -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_control_motor_; -// device::DjiMotor drive_belt_motor_[2]; - -// device::ForceSensorRuntime force_sensor_; -// device::TriggerServo trigger_servo_; - -// // Subscriptions -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; - -// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; -// std::thread event_thread_; - -// // Output interfaces for IMU data -// OutputInterface pitch_angle_; -// OutputInterface final_roll; -// OutputInterface final_pitch; -// OutputInterface final_yaw; - -// // Debug output interfaces for raw IMU data -// OutputInterface debug_raw_roll_; -// OutputInterface debug_raw_pitch_; -// OutputInterface debug_raw_yaw_; - -// // Debug output interfaces (only transformed angles) -// OutputInterface debug_transformed_roll_unnorm_; -// OutputInterface debug_transformed_pitch_unnorm_; -// OutputInterface debug_transformed_yaw_unnorm_; -// OutputInterface debug_transformed_roll_norm_; -// OutputInterface debug_transformed_pitch_norm_; -// OutputInterface debug_transformed_yaw_norm_; - -// // Debug output for drift compensation -// OutputInterface debug_drift_coefficient_; -// OutputInterface debug_calibration_complete_; -// OutputInterface debug_compensation_applied_; - -// // IMU calibration variables -// bool calibration_started_ = false; -// bool calibration_complete_ = false; -// double yaw_drift_coefficient_ = 0.0; -// double yaw_at_first_sample_ = 0.0; -// double yaw_at_final_sample_ = 0.0; -// double first_sample_time_ = 0.0; -// double final_sample_time_ = 0.0; -// }; -// } // namespace rmcs_core::hardware - -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp b/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp deleted file mode 100644 index fb0f7af7..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/test5.cpp +++ /dev/null @@ -1,439 +0,0 @@ -// #include "librmcs/client/cboard.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/bmi088.hpp" -// #include "hardware/device/force_sensor_runtime.hpp" -// #include "hardware/device/trigger_servo.hpp" -// #include -// #include -// #include -// #include -// #include -// #include "filter/low_pass_filter.hpp" -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware { - -// class CatapultDart -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::client::CBoard { -// public: -// CatapultDart() -// : 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()) -// , pitch_angle_filter_(20.0, 1000.0) -// , pitch_velocity_filter_(20.0, 1000.0) -// , yaw_velocity_filter_(20.0, 1000.0) -// , yaw_filter_(5.0, 1000.0) -// , pitch_filter_(10.0, 1000.0) -// , roll_filter_(10.0, 1000.0) -// , dart_command_(create_partner_component(get_component_name() + "_command", *this)) -// , dr16_(*this) -// , imu_(1000, 0.2, 0.0) -// , pitch_motor_( -// *this, *dart_command_, "/dart/pitch_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , yaw_motor_( -// *this, *dart_command_, "/dart/yaw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , force_control_motor_( -// *this, *dart_command_, "/dart/force_control_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , drive_belt_motor_( -// {*this, *dart_command_, "/dart/drive_belt/left", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, -// {*this, *dart_command_, "/dart/drive_belt/right", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508} -// .set_reduction_ratio(19.) -// .set_reversed()}) -// , force_sensor_(*this) -// , trigger_servo_(*dart_command_, "/dart/trigger_servo") -// //, elevating_motor_left_(*dart_command_, "/dart/elevating_motor_left") -// //, elevating_motor_right_(*dart_command_, "/dart/elevating_motor_right") -// //, fill_limiting_servo_(*dart_command_, "/dart/fill_limiting_servo") -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { - -// register_output("/tf", tf_); - -// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); -// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); -// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - -// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); -// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - -// setup_imu_coordinate_mapping(); -// imu_sampler_initialize(); -// tf_broadcaster_ = std::make_unique(*this); -// start_time_ = std::chrono::steady_clock::now(); -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback(std::move(msg)); -// }); -// } - -// ~CatapultDart() override { -// stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); - -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_[0].update_status(); -// drive_belt_motor_[1].update_status(); -// force_control_motor_.update_status(); -// force_sensor_.update_status(); - -// imu_.update_status(); -// processImuData(); -// } - -// void command_update() { -// uint16_t can_commands[4]; - -// if (pub_time_count_++ > 100) { -// transmit_buffer_.add_can1_transmission( -// 0x301, std::bit_cast(force_sensor_.generate_command())); -// pub_time_count_ = 0; -// } - -// can_commands[0] = pitch_motor_.generate_command(); -// can_commands[1] = yaw_motor_.generate_command(); -// can_commands[2] = force_control_motor_.generate_command(); -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - -// can_commands[0] = drive_belt_motor_[0].generate_command(); -// can_commands[1] = drive_belt_motor_[1].generate_command(); -// can_commands[2] = 0; -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - -// if (!trigger_servo_.calibrate_mode()) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); -// } - -// transmit_buffer_.trigger_transmission(); -// } -// int pub_time_count_ = 0; - -// 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 == 0x302) { -// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] -// return; - -// if (can_id == 0x201) { -// pitch_motor_.store_status(can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(can_data); -// } else if (can_id == 0x203) { -// force_control_motor_.store_status(can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_[0].store_status(can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_[1].store_status(can_data); -// } -// } - -// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_accelerometer_status(x, y, z); -// } - -// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_gyroscope_status(x, y, z); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!success) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// } - -// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { -// dr16_.store_status(uart_data, uart_data_length); -// } - -// private: -// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { -// trigger_servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } - -// void setup_imu_coordinate_mapping() { -// imu_.set_coordinate_mapping( -// [](double x, double y, double z) -> std::tuple { -// return {x, -y, -z}; -// }); -// } - -// void imu_sampler_initialize() { -// start_time_ = std::chrono::steady_clock::now(); -// calibration_started_ = false; -// calibration_complete_ = false; -// yaw_drift_coefficient_ = 0.0; -// first_sample_taken_ = false; -// final_sample_taken_ = false; - -// } - -// void processImuData() { -// auto current_time = std::chrono::steady_clock::now(); -// double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); - -// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - -// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - -// double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); -// double pitch = std::asin(-rotation_matrix(2,0)); -// double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); - -// double transformed_roll = -roll_filter_.update(roll); -// double transformed_pitch = pitch_filter_.update(pitch); -// double transformed_yaw = -yaw_filter_.update(yaw); - -// *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; -// *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; -// *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; - -// transformed_roll = normalizeAngle(transformed_roll); -// transformed_pitch = normalizeAngle(transformed_pitch); -// transformed_yaw = normalizeAngle(transformed_yaw); - -// double filtered_roll = roll_filter_.update(transformed_roll); -// double filtered_pitch = pitch_filter_.update(transformed_pitch); -// double filtered_yaw = yaw_filter_.update(transformed_yaw); - -// if (!calibration_complete_) { -// if (elapsed_seconds >= final_sample_spot_ && !final_sample_taken_) { -// double yaw_at_first_sample = filtered_yaw; -// double yaw_at_final_sample = filtered_yaw; -// double final_sample_time = elapsed_seconds; -// final_sample_taken_ = true; - -// double time_diff = final_sample_time - first_sample_spot_; -// double yaw_diff = yaw_at_final_sample - yaw_at_first_sample; - -// if (time_diff > 0.1) { -// yaw_drift_coefficient_ = yaw_diff / time_diff; -// calibration_complete_ = true; -// } -// } -// } - -// double compensated_yaw = filtered_yaw; - -// if (calibration_complete_) { -// double time_since_calibration = elapsed_seconds - first_sample_spot_; - -// compensated_yaw = filtered_yaw - (yaw_drift_coefficient_ * time_since_calibration); - -// compensated_yaw = normalizeAngle(compensated_yaw); -// } - -// publishTfTransforms(filtered_roll, filtered_pitch, compensated_yaw); -// } - -// static double normalizeAngle(double angle) { -// while (angle > M_PI) angle -= 2.0 * M_PI; -// while (angle < -M_PI) angle += 2.0 * M_PI; -// return angle; -// } - -// void publishTfTransforms(double roll, double pitch, double yaw) { -// auto now = this->get_clock()->now(); - -// auto create_transform = [&](const std::string& parent, const std::string& child, -// const geometry_msgs::msg::Vector3& trans, -// const geometry_msgs::msg::Quaternion& rot) { -// geometry_msgs::msg::TransformStamped tf; -// tf.header.stamp = now; -// tf.header.frame_id = parent; -// tf.child_frame_id = child; -// tf.transform.translation = trans; -// tf.transform.rotation = rot; -// return tf; -// }; - -// auto create_translation = [](double x, double y, double z) { -// geometry_msgs::msg::Vector3 t; -// t.x = x; t.y = y; t.z = z; -// return t; -// }; - -// auto create_rotation = [](double x, double y, double z, double w) { -// geometry_msgs::msg::Quaternion r; -// r.x = x; r.y = y; r.z = z; r.w = w; -// return r; -// }; -// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); -// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - -// // Create quaternions for each axis rotation -// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); -// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); -// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - -// // Combined rotation: yaw -> pitch -> roll -// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - -// // Publish all TF transforms -// tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", -// zero_trans, create_rotation(0, 0, 0, 1))); - -// tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", -// zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), -// yaw_quat.z(), yaw_quat.w()))); - -// tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", -// pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), -// pitch_quat.z(), pitch_quat.w()))); - -// tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", -// zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), -// combined_quaternion.z(), combined_quaternion.w()))); - -// tf_broadcaster_->sendTransform(create_transform("world", "base_link", -// zero_trans, create_rotation(0, 0, 0, 1))); -// } - -// rclcpp::Logger logger_; - -// filter::LowPassFilter<1> pitch_angle_filter_; -// filter::LowPassFilter<1> pitch_velocity_filter_; -// filter::LowPassFilter<1> yaw_velocity_filter_; -// filter::LowPassFilter<1> yaw_filter_; -// filter::LowPassFilter<1> pitch_filter_; -// filter::LowPassFilter<1> roll_filter_; - -// std::chrono::steady_clock::time_point start_time_; - -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDart& robot) -// : dart_(robot) {} - -// void update() override { dart_.command_update(); } - -// CatapultDart& dart_; -// }; -// std::shared_ptr dart_command_; -// double first_sample_spot_; -// double final_sample_spot_; - -// OutputInterface tf_; -// std::unique_ptr tf_broadcaster_; - -// device::Dr16 dr16_; -// device::Bmi088 imu_; -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_control_motor_; -// device::DjiMotor drive_belt_motor_[2]; - -// device::ForceSensorRuntime force_sensor_; -// device::TriggerServo trigger_servo_; -// // device::TriggerServo elevating_motor_left_; -// // device::TriggerServo elevating_motor_right_; -// // device::TriggerServo fill_limiting_servo_; - -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; - -// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; -// std::thread event_thread_; - -// OutputInterface catapult_pitch_angle_; -// OutputInterface catapult_roll_angle_; -// OutputInterface catapult_yaw_angle_; - -// bool calibration_started_ = false; -// bool calibration_complete_ = false; -// bool first_sample_taken_ = false; -// bool final_sample_taken_ = false; -// double yaw_drift_coefficient_ = 0.0; -// }; -// } // namespace rmcs_core::hardware - -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt b/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt new file mode 100644 index 00000000..a2b3f6a1 --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt @@ -0,0 +1,97 @@ +cmake_minimum_required(VERSION 3.12) +project(rmcs_dart_guidance) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_BUILD_TYPE "Release") +add_compile_options(-Wall -Wextra -Wpedantic -O3) + +find_package(ament_cmake REQUIRED) + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(type_description_interfaces REQUIRED) + +find_package(OpenCV 4.5 REQUIRED) +find_package(OpenVINO REQUIRED) +find_package(Ceres REQUIRED) + +find_package(rmcs_executor REQUIRED) +find_package(rmcs_description REQUIRED) +find_package(fast_tf REQUIRED) +find_package(hikcamera REQUIRED) + +include_directories( + ${PROJECT_SOURCE_DIR}/src/ + ${EIGEN3_INCLUDE_DIRS} + ${type_description_interfaces_INCLUDE_DIRS} + ${rclcpp_INCLUDE_DIRS} + ${rmcs_executor_INCLUDE_DIRS} + ${rmcs_description_INCLUDE_DIRS} + ${fast_tf_INCLUDE_DIRS} + ${hikcamera_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${OpenVINO_INCLUDE_DIRS} + ${Ceres_INCLUDE_DIRS} +) + +file(GLOB_RECURSE PROJECT_SOURCE + CONFIGURE_DEPENDS + ${PROJECT_SOURCE_DIR}/src/*.cpp + ${PROJECT_SOURCE_DIR}/src/*.cc + ${PROJECT_SOURCE_DIR}/test/*.cpp + ${PROJECT_SOURCE_DIR}/test/*.cc +) + +add_library( + ${PROJECT_NAME} SHARED + ${PROJECT_SOURCE} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + sensor_msgs + cv_bridge + geometry_msgs + visualization_msgs +) + +target_link_libraries( + ${PROJECT_NAME} + ${rclcpp_LIBRARIES} + ${std_msgs_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${cv_bridge_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${visualization_msgs_LIBRARIES} + ${type_description_interfaces_LIBRARIES} + + ${OpenCV_LIBS} + ${OpenVINO_LIBRARIES} + ${Ceres_LIBRARIES} + ${rmcs_executor_LIBRARIES} + ${rmcs_description_LIBRARIES} + ${fast_tf_LIBRARIES} + ${hikcamera_LIBRARIES} +) + +install( + TARGETS + ${PROJECT_NAME} + DESTINATION + lib/ +) + +find_package(pluginlib REQUIRED) +pluginlib_export_plugin_description_file( + rmcs_executor plugins.xml +) + +ament_package() \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/package.xml b/rmcs_ws/src/rmcs_dart_guidance/package.xml new file mode 100644 index 00000000..474d59ab --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/package.xml @@ -0,0 +1,32 @@ + + + + rmcs_dart_guidance + 0.0.0 + TODO: Package description + ubuntu + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + pluginlib + hikcamera + rmcs_executor + rmcs_description + fast_tf + sensor_msgs + cv_bridge + geometry_msgs + tf2_ros + tf2_geometry_msgs + eigen3_cmake_module + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rmcs_ws/src/rmcs_dart_guidance/plugins.xml b/rmcs_ws/src/rmcs_dart_guidance/plugins.xml new file mode 100644 index 00000000..4f1ad0f0 --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/plugins.xml @@ -0,0 +1,21 @@ + + + + Image processing node. + + + test node. + + + test node. + + + test node. + + + test node. + + + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp new file mode 100644 index 00000000..32de6fbe --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp @@ -0,0 +1,142 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class DartGuidanceAngleSolver { +public: + DartGuidanceAngleSolver() { Init(); } + + void Init() { + focal_length_x_ = 800.0; + focal_length_y_ = 800.0; + principal_point_x_ = 320.0; + principal_point_y_ = 240.0; + target_real_area_ = 0.0; + is_intrinsics_set_ = false; + is_extrinsic_set_ = false; + cached_target_area_ = 0.0; + cache_valid_ = false; + } + + void set_default(double flx, double fly, double ppx, double ppy, double real_area) { + focal_length_x_ = flx; + focal_length_y_ = fly; + principal_point_x_ = ppx; + principal_point_y_ = ppy; + target_real_area_ = real_area; + is_intrinsics_set_ = true; + } + + void set_extrinsic(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) { + R_camera2launcher_ = R; + t_camera2launcher_ = t; + is_extrinsic_set_ = true; + } + + void set_cached_target_area(double area) { + cached_target_area_ = area; + cache_valid_ = (area > 1e-8); + } + + double estimate_distance(double pixel_area) const { + if (pixel_area <= 1e-8 || target_real_area_ <= 0 || !is_intrinsics_set_) + return -1.0; + return std::sqrt(target_real_area_ * focal_length_x_ * focal_length_y_ / pixel_area); + } + + Eigen::Vector2d calculate_angles_error_simple(const cv::Point2i& pixel) const { + double dx = pixel.x - principal_point_x_; + double dy = pixel.y - principal_point_y_; + double yaw = std::atan2(dx, focal_length_x_); + double pitch = std::atan2(dy, focal_length_y_); + return {yaw, pitch}; + } + + Eigen::Vector2d compute_angles_error_accurate(const cv::Point2i& pixel, double distance) const { + if (!is_intrinsics_set_ || !is_extrinsic_set_ || distance <= 0) + return Eigen::Vector2d::Zero(); + + double Zc = distance; + double Xc = (pixel.x - principal_point_x_) * Zc / focal_length_x_; + double Yc = (pixel.y - principal_point_y_) * Zc / focal_length_y_; + Eigen::Vector3d P_camera(Xc, Yc, Zc); + Eigen::Vector3d P_launcher = R_camera2launcher_ * P_camera + t_camera2launcher_; + + double x = P_launcher.x(); + double y = P_launcher.y(); + double z = P_launcher.z(); + + double yaw = std::atan2(x, z); + double pitch = std::atan2(y, std::sqrt(x*x + y*y)); + return {yaw, pitch}; + } + + Eigen::Vector2d update(const cv::Point2i& pixel, double pixel_area, bool use_accurate = true) { + last_pixel_ = pixel; + last_area_ = pixel_area; + last_distance_ = estimate_distance(pixel_area); + + if (use_accurate && is_extrinsic_set_ && last_distance_ > 0) { + last_angle_error_ = compute_angles_error_accurate(pixel, last_distance_); + } else { + last_angle_error_ = calculate_angles_error_simple(pixel); + } + + last_process_time_ = std::chrono::steady_clock::now(); + return last_angle_error_; + } + + Eigen::Vector2d update(const cv::Point2i& pixel, bool use_accurate = true) { + last_pixel_ = pixel; + if (cache_valid_ && target_real_area_ > 0 && is_intrinsics_set_) { + last_distance_ = estimate_distance(cached_target_area_); + } else { + last_distance_ = -1.0; + } + + if (use_accurate && is_extrinsic_set_ && last_distance_ > 0) { + last_angle_error_ = compute_angles_error_accurate(pixel, last_distance_); + } else { + last_angle_error_ = calculate_angles_error_simple(pixel); + } + + last_process_time_ = std::chrono::steady_clock::now(); + return last_angle_error_; + } + + Eigen::Vector2d getLastAngleError() const { return last_angle_error_; } + double getLastDistance() const { return last_distance_; } + cv::Point2i getLastPixel() const { return last_pixel_; } + double getLastArea() const { return last_area_; } + bool isIntrinsicsSet() const { return is_intrinsics_set_; } + bool isExtrinsicSet() const { return is_extrinsic_set_; } + +private: + bool is_intrinsics_set_ = false; + bool is_extrinsic_set_ = false; + double focal_length_x_; + double focal_length_y_; + double principal_point_x_; + double principal_point_y_; + double target_real_area_; + + Eigen::Matrix3d R_camera2launcher_; + Eigen::Vector3d t_camera2launcher_; + + double cached_target_area_ = 0.0; + bool cache_valid_ = false; + + mutable cv::Point2i last_pixel_ = cv::Point2i(-1, -1); + mutable double last_area_ = 0.0; + mutable double last_distance_ = 0.0; + mutable Eigen::Vector2d last_angle_error_ = Eigen::Vector2d::Zero(); + mutable std::chrono::steady_clock::time_point last_process_time_; +}; + +} // namespace rmcs_dart_guidance \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp new file mode 100644 index 00000000..10980afe --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp @@ -0,0 +1,341 @@ +#include "identifier.hpp" +#include "tracker.hpp" +#include "angle_solver.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class DartVisionCore + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartVisionCore() + : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + camera_profile_.invert_image = get_parameter("invert_image").as_bool(); + camera_profile_.exposure_time = std::chrono::microseconds(get_parameter("exposure_time").as_int()); + camera_profile_.gain = static_cast(get_parameter("gain").as_double()); + + lower_limit_default_ = cv::Scalar(get_parameter("L_H").as_double(), get_parameter("L_S").as_double(), get_parameter("L_V").as_double()); + upper_limit_default_ = cv::Scalar(get_parameter("U_H").as_double(), get_parameter("U_S").as_double(), get_parameter("U_V").as_double()); + + enable_image_saving_ = has_parameter("enable_image_saving") ? + get_parameter("enable_image_saving").as_bool() : false; + save_directory_ = has_parameter("image_save_directory") ? + get_parameter("image_save_directory").as_string() : "./saved_images"; + save_interval_ms_ = has_parameter("image_save_interval_ms") ? + static_cast(get_parameter("image_save_interval_ms").as_int()) : 1000; + save_raw_image_ = has_parameter("save_raw_image") ? + get_parameter("save_raw_image").as_bool() : true; + save_processed_image_ = has_parameter("save_processed_image") ? + get_parameter("save_processed_image").as_bool() : false; + + double fx_ = get_parameter_or("camera_focal_length_x", 4242.6083); + double fy_ = get_parameter_or("camera_focal_length_y", 4227.9942); + double cx_ = get_parameter_or("camera_principal_point_x", 632.9298); + double cy_ = get_parameter_or("camera_principal_point_y", 556.6188); + + double target_diameter_ = get_parameter_or("target_diameter", 0.1); + double target_area_ = CV_PI * std::pow(target_diameter_ / 2.0, 2); + + std::vector R_vec = get_parameter_or("cam2launcher_R", + std::vector{1,0,0,0,1,0,0,0,1}); + std::vector T_vec = get_parameter_or("cam2launcher_T", + std::vector{0,0,0}); + Eigen::Matrix3d R = Eigen::Map>(R_vec.data()); + Eigen::Vector3d t = Eigen::Map(T_vec.data()); + angle_solver_.set_default(fx_, fy_, cx_, cy_, target_area_); + angle_solver_.set_extrinsic(R, t); + angle_solver_.set_cached_target_area(0.0); + + image_capture_ = std::make_unique(camera_profile_); + + identifier_.set_default_limit(lower_limit_default_, upper_limit_default_); + identifier_.Init(); + + if (enable_image_saving_) { + try { + std::filesystem::create_directories(save_directory_); + RCLCPP_INFO(logger_, "Created image save directory: %s", save_directory_.c_str()); + + test_write_permission(); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Failed to create save directory %s: %s", + save_directory_.c_str(), e.what()); + enable_image_saving_ = false; + } + } + + register_output("/dart_guidance/camera/camera_image", camera_image_); + register_output("/dart_guidance/camera/display_image", display_image_); + register_output("/dart_guidance/camera/target_position", target_position_, PointT(-1, -1)); + register_output("/dart_guidance/tracker/tracking", tracking_); + register_output("/dart_guidance/angle/error", angle_error_); + if (enable_image_saving_) { + RCLCPP_INFO(logger_, "Image saving enabled:"); + RCLCPP_INFO(logger_, " Directory: %s", save_directory_.c_str()); + RCLCPP_INFO(logger_, " Interval: %d ms", save_interval_ms_); + RCLCPP_INFO(logger_, " Save raw: %s", save_raw_image_ ? "true" : "false"); + RCLCPP_INFO(logger_, " Save processed: %s", save_processed_image_ ? "true" : "false"); + } else { + RCLCPP_INFO(logger_, "Image saving disabled"); + } + + camera_thread_ = std::thread(&DartVisionCore::camera_update, this); + update_time_point_ = std::chrono::steady_clock::now(); + } + + ~DartVisionCore() { + if (camera_thread_.joinable()) { + camera_thread_.join(); + } + } + + void update() override {} + + std::chrono::steady_clock::time_point update_time_point_; + +private: + void camera_update() { + int frame_counter = 0; + int saved_raw_counter = 0; + int saved_processed_counter = 0; + + std::chrono::steady_clock::time_point last_save_time = std::chrono::steady_clock::now(); + + while (rclcpp::ok()) { + try { + frame_counter++; + + cv::Mat raw_image = image_capture_->read(); + if (raw_image.empty()) { + RCLCPP_WARN_THROTTLE(logger_, *this->get_clock(), 1000, + "Received empty image from camera"); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + + cv::line(raw_image, cv::Point(645, 0), cv::Point(645, 720), cv::Scalar(255, 0, 255), 1); + + *camera_image_ = raw_image; + + auto now = std::chrono::steady_clock::now(); + auto elapsed_ms = std::chrono::duration_cast( + now - last_save_time).count(); + + if (enable_image_saving_ && save_raw_image_ && elapsed_ms >= save_interval_ms_) { + if (save_image(raw_image, "raw")) { + saved_raw_counter++; + RCLCPP_INFO(logger_, "Saved raw image %d", saved_raw_counter); + last_save_time = now; + } + } + + cv::Mat preprocessed_image; + image_to_binary(raw_image, preprocessed_image); + + cv::Mat display_image = preprocessed_image.clone(); + process_frame(preprocessed_image, display_image); + + cv::line(display_image, cv::Point(0, 645), cv::Point(720, 645), cv::Scalar(255, 0, 255), 1); + *display_image_ = display_image; + + if (enable_image_saving_ && save_processed_image_ && elapsed_ms >= save_interval_ms_) { + if (save_image(display_image, "processed")) { + saved_processed_counter++; + RCLCPP_INFO(logger_, "Saved processed image %d", saved_processed_counter); + } + } + + if (frame_counter % 30 == 0) { + RCLCPP_DEBUG(logger_, "Processed %d frames", frame_counter); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Error in camera_update: %s", e.what()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + + RCLCPP_INFO(logger_, "Camera thread stopped. Frames: %d, Raw saved: %d, Processed saved: %d", + frame_counter, saved_raw_counter, saved_processed_counter); + } + + bool save_image(const cv::Mat& image, const std::string& type) { + try { + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf; + localtime_r(&time_t, &tm_buf); + + std::ostringstream oss; + oss << save_directory_ << "/" << type + << "_" << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") + << "_" << std::setw(4) << std::setfill('0') << save_counter_++ + << ".jpg"; + + std::string filename = oss.str(); + + bool success = cv::imwrite(filename, image); + if (success) { + if (std::filesystem::exists(filename)) { + return true; + } else { + RCLCPP_ERROR(logger_, "File not created: %s", filename.c_str()); + } + } else { + RCLCPP_ERROR(logger_, "cv::imwrite failed for: %s", filename.c_str()); + } + } catch (const cv::Exception& e) { + RCLCPP_ERROR(logger_, "OpenCV error saving image: %s", e.what()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Error saving image: %s", e.what()); + } + + return false; + } + + void test_write_permission() { + std::string test_file = save_directory_ + "/test_write.jpg"; + cv::Mat test_image(100, 100, CV_8UC3, cv::Scalar(255, 0, 0)); + + try { + bool success = cv::imwrite(test_file, test_image); + if (success && std::filesystem::exists(test_file)) { + auto file_size = std::filesystem::file_size(test_file); + RCLCPP_INFO(logger_, "Write test successful: %s (%ld bytes)", test_file.c_str(), file_size); + + std::filesystem::remove(test_file); + } else { + RCLCPP_ERROR(logger_, "Write test failed: %s", test_file.c_str()); + } + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Write test exception: %s", e.what()); + } + } + + void process_frame(cv::Mat& preprocessed_image, cv::Mat& display_image) { + if (!is_tracker_stage_) { + identifier_.update(preprocessed_image); + + cv::putText( + display_image, "Identifying", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 255), 2); + + if (!identifier_.result_status_()) { + *target_position_ = PointT(-1, -1); + *tracking_ = false; + *angle_error_ = Eigen::Vector2d::Zero(); + } else { + cv::Point2i initial_position = identifier_.get_result(); + RCLCPP_INFO(logger_, "Target initial position:(%d,%d)", initial_position.x, initial_position.y); + + double init_area = identifier_.get_target_area(); + angle_solver_.set_cached_target_area(init_area); + is_tracker_stage_ = true; + tracker_.Init(initial_position); + + *target_position_ = initial_position; + *tracking_ = true; + *angle_error_ = angle_solver_.update(initial_position, true); + } + } else { + tracker_.update(preprocessed_image); + cv::Point2i current_position = tracker_.get_current_position(); + + bool is_tracking = tracker_.get_tracking_status(); + cv::putText( + display_image, is_tracking ? "Tracking" : "Lost", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 255), 2); + + if (!is_tracking) { + tracker_loss_count_++; + *target_position_ = PointT(-1, -1); + *tracking_ = false; + *angle_error_ = Eigen::Vector2d::Zero(); + + if (tracker_loss_count_ > 100) { + is_tracker_stage_ = false; + identifier_.Init(); + angle_solver_.set_cached_target_area(0.0); + tracker_loss_count_ = 0; + RCLCPP_INFO(logger_, "Target lost, switching to identification mode"); + } + } else { + tracker_loss_count_ = 0; + *target_position_ = current_position; + *tracking_ = true; + *angle_error_ = angle_solver_.update(current_position, true); + cv::circle(display_image, current_position, 20, cv::Scalar(255, 0, 255), 2); + } + } + } + + void image_to_binary(const cv::Mat& src, cv::Mat& output) { + cv::Mat HSV_image; + cv::cvtColor(src, HSV_image, cv::COLOR_BGR2HSV); + + cv::Mat binary; + cv::inRange(HSV_image, lower_limit_default_, upper_limit_default_, binary); + + static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(9, 9)); + cv::morphologyEx(binary, binary, cv::MORPH_OPEN, kernel); + cv::morphologyEx(binary, binary, cv::MORPH_CLOSE, kernel); + output = binary; + } + + rclcpp::Logger logger_; + + bool enable_image_saving_ = false; + std::string save_directory_ = "./saved_images"; + int save_interval_ms_ = 1000; + bool save_raw_image_ = true; + bool save_processed_image_ = false; + int save_counter_ = 0; + + std::thread camera_thread_; + std::mutex camera_thread_mtx_; + + cv::Scalar lower_limit_default_, upper_limit_default_; + + hikcamera::ImageCapturer::CameraProfile camera_profile_; + std::unique_ptr image_capture_; + + OutputInterface camera_image_; + OutputInterface display_image_; + OutputInterface tracking_; + OutputInterface target_position_; + OutputInterface angle_error_; + + DartGuidanceIdentifier identifier_; + DartGuidanceTracker tracker_; + DartGuidanceAngleSolver angle_solver_; + bool is_tracker_stage_ = false; + int tracker_loss_count_ = 0; +}; +} // namespace rmcs_dart_guidance + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::DartVisionCore, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp new file mode 100644 index 00000000..64f2b284 --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp @@ -0,0 +1,191 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace rmcs_dart_guidance { + +using PointT = cv::Point2i; + +struct TargetData { + PointT first_position; + PointT latest_position; + double max_move_distance; + int catch_count; + int miss_count; + double area; +}; + +class DartGuidanceIdentifier { +public: + DartGuidanceIdentifier() { + Init(); + } + + void Init() { + detect_frame_count_ = 0; + result_ready_ = false; + possible_targets_collection_.clear(); + start_time_ = std::chrono::steady_clock::now(); + last_target_area_ = 0.0; + } + + void set_default_limit(const cv::Scalar& lower, const cv::Scalar& upper) { + lower_limit_ = lower; + upper_limit_ = upper; + } + + void update(const cv::Mat& binary_image) { + if (result_ready_) { + Init(); + } + + std::vector> possible_targets = process(binary_image); + target_filter(possible_targets); + + detect_frame_count_++; + + if (detect_frame_count_ < 80) { + return; + } + + double highest_score = 0; + size_t most_possible_target_id = -1; + + if (possible_targets_collection_.empty()) { + Init(); + } else { + for (size_t i = 0; i < possible_targets_collection_.size(); i++) { + double this_score = possible_targets_collection_[i].catch_count + + 1000 / (possible_targets_collection_[i].max_move_distance + 1); + + if (this_score > highest_score) { + most_possible_target_id = i; + highest_score = this_score; + } + } + target_initial_position_ = possible_targets_collection_[most_possible_target_id].latest_position; + last_target_area_ = possible_targets_collection_[most_possible_target_id].area; + result_ready_ = true; + + auto end_time_ = std::chrono::steady_clock::now(); + auto delta_time = std::chrono::duration_cast(end_time_ - start_time_).count(); + last_process_time_ms_ = delta_time; + } + } + + cv::Mat get_display_image() { return display_image_; } + bool result_status_() const { return result_ready_; } + PointT get_result() { return target_initial_position_; } + double get_target_area() const { return last_target_area_; } + + std::string get_debug_info() const { + std::string info = "Identifier: Frames=" + std::to_string(detect_frame_count_) + + ", Targets=" + std::to_string(possible_targets_collection_.size()) + + ", Result=" + (result_ready_ ? "Ready" : "Not Ready"); + if (result_ready_) { + info += ", Pos=(" + std::to_string(target_initial_position_.x) + + "," + std::to_string(target_initial_position_.y) + ")"; + } + if (last_process_time_ms_ > 0) { + info += ", Time=" + std::to_string(last_process_time_ms_) + "ms"; + } + return info; + } + +private: + + cv::Mat display_image_; + cv::Scalar lower_limit_, upper_limit_; + cv::Scalar latest_lower_limit_, latest_upper_limit_; + std::vector possible_targets_collection_; + int detect_frame_count_; + PointT target_initial_position_; + bool result_ready_; + + std::chrono::steady_clock::time_point start_time_; + int64_t last_process_time_ms_ = 0; + double last_target_area_ = 0.0; + + static std::vector> process(const cv::Mat& binary_image) { + std::vector> contours; + std::vector hierarchy; + cv::findContours(binary_image, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + std::vector> possible_targets; + for (const auto& contour : contours) { + double area = cv::contourArea(contour); + if (area < 64 || area > 5000) continue; + if (contour.size() < 5) continue; + + cv::Point2f circle_center; + float circle_radius; + cv::minEnclosingCircle(contour, circle_center, circle_radius); + double enclosing_circle_area = CV_PI * circle_radius * circle_radius; + double area_ratio = 0; + if (enclosing_circle_area > 0) { + area_ratio = area / enclosing_circle_area; + } + if (area_ratio >= 0.8) { + possible_targets.emplace_back(cv::Point2i(circle_center), area); + } + } + return possible_targets; + } + + void target_filter(const std::vector>& points) { + const int distance_threshold = 10; + std::vector matched(points.size(), false); + + for (auto& collected: possible_targets_collection_) { + double min_distance = std::numeric_limits::max(); + int point_id = -1; + + for (size_t i = 0; i < points.size(); ++i) { + if (matched[i]) continue; + + double distance = cv::norm(points[i].first - collected.latest_position); + if (distance < distance_threshold && distance < min_distance) { + min_distance = distance; + point_id = static_cast(i); + } + } + + if (point_id != -1) { + double this_move_distance = cv::norm(collected.first_position - points[point_id].first); + collected.max_move_distance = std::max(collected.max_move_distance, this_move_distance); + collected.latest_position = points[point_id].first; + collected.area = points[point_id].second; + collected.catch_count++; + collected.miss_count = 0; + matched[point_id] = true; + } else { + collected.miss_count++; + } + } + + for (size_t i = 0; i < points.size(); ++i) { + if (!matched[i]) { + TargetData new_target_data(points[i].first, points[i].first, 0, 1, 0, points[i].second); + possible_targets_collection_.emplace_back(new_target_data); + } + } + + possible_targets_collection_.erase( + std::remove_if( + possible_targets_collection_.begin(), + possible_targets_collection_.end(), + [](const TargetData& target) { return target.miss_count >= 20; } + ), + possible_targets_collection_.end() + ); + } +}; +} \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp new file mode 100644 index 00000000..9ce0948f --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp @@ -0,0 +1,112 @@ +#pragma once + +#include +#include + +namespace rmcs_dart_guidance { +class DartGuidanceTracker{ +public: + explicit DartGuidanceTracker(double min_contour_area = 20.0, double max_distance_threshold = 200.0) + : current_position_(-1, -1) + , is_initialized_(false) + , min_contour_area_(min_contour_area) + , max_distance_threshold_(max_distance_threshold) {} + + void Init(cv::Point2i& initial_position) { + current_position_ = initial_position; + is_initialized_ = true; + tracking_count_ = 0; + lost_count_ = 0; + last_track_time_ = std::chrono::steady_clock::now(); + } + + void update(const cv::Mat& binary_image) { + auto start_time = std::chrono::steady_clock::now(); + + if (!is_initialized_) { + return; + } + + if (binary_image.empty() || binary_image.channels() != 1 || binary_image.type() != CV_8U) { + return; + } + + std::vector> contours; + cv::findContours(binary_image, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); + + double min_distance = std::numeric_limits::max(); + cv::Point2i best_centroid = current_position_; + bool found_match = false; + + + for (const auto& contour : contours) { + double area = cv::contourArea(contour); + if (area < min_contour_area_) { + continue; + } + + cv::Moments moments = cv::moments(contour); + + if (moments.m00 == 0) { + continue; + } + + cv::Point2f centroid(static_cast(moments.m10 / moments.m00), static_cast(moments.m01 / moments.m00)); + + double distance = cv::norm(centroid - (cv::Point2f)current_position_); + + if (distance < min_distance) { + min_distance = distance; + best_centroid = centroid; + found_match = true; + } + + valid_contours_++; + } + + if (found_match && min_distance < max_distance_threshold_) { + current_position_ = cv::Point2i(static_cast(std::round(best_centroid.x)), static_cast(std::round(best_centroid.y))); + tracking_flag_ = true; + tracking_count_++; + lost_count_ = 0; + } else { + tracking_flag_ = false; + tracking_count_++; + } + + last_process_time_ms_ = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time).count(); + } + + cv::Point2i get_current_position() const { return current_position_; } + + bool get_tracking_status() const { return tracking_flag_; } + + std::string get_debug_info() const { + std::string info = "Tracker: "; + info += tracking_flag_ ? "Tracking" : "Lost"; + info += ", Pos=(" + std::to_string(current_position_.x) + + "," + std::to_string(current_position_.y) + ")"; + info += ", Tracked=" + std::to_string(tracking_count_); + info += ", Lost=" + std::to_string(lost_count_); + info += ", ValidContours=" + std::to_string(valid_contours_); + if (last_process_time_ms_ > 0) { + info += ", Time=" + std::to_string(last_process_time_ms_) + "ms"; + } + return info; + } + +private: + bool tracking_flag_; + cv::Point2i current_position_; + bool is_initialized_; + double min_contour_area_; + double max_distance_threshold_; + + int tracking_count_ = 0; + int lost_count_ = 0; + int valid_contours_ = 0; + int64_t last_process_time_ms_ = 0; + std::chrono::steady_clock::time_point last_track_time_; +}; +} // namespace rmcs_dart_guidance \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp new file mode 100644 index 00000000..8314a19c --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp @@ -0,0 +1,321 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class DebugDisplayComponent + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DebugDisplayComponent() + : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + // Store parameters in member variables for later use + raw_image_topic_ = get_parameter("raw_image_topic").as_string(); + processed_image_topic_ = get_parameter("processed_image_topic").as_string(); + target_topic_ = get_parameter("target_topic").as_string(); + + display_raw_ = get_parameter("display_raw").as_bool(); + display_processed_ = get_parameter("display_processed").as_bool(); + max_fps_ = static_cast(get_parameter("max_fps").as_int()); + window_scale_ = get_parameter("window_scale").as_double(); + save_on_error_ = get_parameter("save_on_error").as_bool(); + save_directory_ = get_parameter("save_directory").as_string(); + + // Check if display is available + const char* display_env = std::getenv("DISPLAY"); + if (!display_env || std::string(display_env).empty()) { + RCLCPP_WARN(logger_, "DISPLAY environment variable not set. Display windows disabled."); + display_available_ = false; + } else { + display_available_ = true; + } + + RCLCPP_INFO(logger_, "DebugDisplayComponent constructed"); + } + + ~DebugDisplayComponent() { + stop_display(); + } + + void update() override { + // Initialize subscriptions and display on first update + if (!initialized_) { + initialize(); + initialized_ = true; + } + } + +private: + void initialize() { + RCLCPP_INFO(logger_, "Initializing DebugDisplayComponent"); + + // Create subscriptions + if (display_raw_) { + raw_image_sub_ = this->create_subscription( + raw_image_topic_, 10, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + raw_image_callback(msg); + }); + RCLCPP_INFO(logger_, "Subscribed to raw image topic: %s", raw_image_topic_.c_str()); + } + + if (display_processed_) { + processed_image_sub_ = this->create_subscription( + processed_image_topic_, 10, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + processed_image_callback(msg); + }); + RCLCPP_INFO(logger_, "Subscribed to processed image topic: %s", processed_image_topic_.c_str()); + + target_sub_ = this->create_subscription( + target_topic_, 10, + [this](const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { + target_callback(msg); + }); + RCLCPP_INFO(logger_, "Subscribed to target topic: %s", target_topic_.c_str()); + } + + // Start display thread if available + if (display_available_) { + display_running_ = true; + display_thread_ = std::thread(&DebugDisplayComponent::display_loop, this); + RCLCPP_INFO(logger_, "Display thread started"); + } + } + + void stop_display() { + display_running_ = false; + if (display_thread_.joinable()) { + display_thread_.join(); + RCLCPP_INFO(logger_, "Display thread stopped"); + } + + // Clean up OpenCV windows + if (display_raw_) { + cv::destroyWindow("Raw Image"); + } + if (display_processed_) { + cv::destroyWindow("Processed Image"); + } + } + + void raw_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { + std::lock_guard lock(raw_mutex_); + try { + raw_image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; + raw_image_timestamp_ = std::chrono::steady_clock::now(); + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(logger_, "cv_bridge exception for raw image: %s", e.what()); + } + } + + void processed_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + std::lock_guard lock(processed_mutex_); + try { + processed_image_ = cv_bridge::toCvCopy(msg, "mono8")->image; + processed_image_timestamp_ = std::chrono::steady_clock::now(); + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(logger_, "cv_bridge exception for processed image: %s", e.what()); + } + } + + void target_callback(const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { + std::lock_guard lock(target_mutex_); + target_position_.x = static_cast(msg->point.x); + target_position_.y = static_cast(msg->point.y); + target_tracking_ = (msg->point.z > 0.5); + last_target_time_ = std::chrono::steady_clock::now(); + } + + void display_loop() { + // Create display windows + if (display_raw_) { + cv::namedWindow("Raw Image", cv::WINDOW_NORMAL); + cv::resizeWindow("Raw Image", + static_cast(640 * window_scale_), + static_cast(480 * window_scale_)); + } + + if (display_processed_) { + cv::namedWindow("Processed Image", cv::WINDOW_NORMAL); + cv::resizeWindow("Processed Image", + static_cast(640 * window_scale_), + static_cast(480 * window_scale_)); + } + + const int frame_delay_ms = 1000 / std::max(1, max_fps_); + int frame_counter = 0; + auto last_fps_time = std::chrono::steady_clock::now(); + + RCLCPP_INFO(logger_, "Display loop started with delay %d ms", frame_delay_ms); + + while (display_running_ && rclcpp::ok()) { + try { + // Display raw image + if (display_raw_) { + cv::Mat display_raw; + { + std::lock_guard lock(raw_mutex_); + if (!raw_image_.empty()) { + display_raw = raw_image_.clone(); + + // Check for stale data + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast( + now - raw_image_timestamp_).count(); + + if (elapsed > 500) { + cv::putText(display_raw, "STALE", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); + } + } + } + + if (!display_raw.empty()) { + cv::imshow("Raw Image", display_raw); + } + } + + // Display processed image + if (display_processed_) { + cv::Mat display_processed; + { + std::lock_guard lock(processed_mutex_); + if (!processed_image_.empty()) { + display_processed = processed_image_.clone(); + + // Convert to color for visualization + if (display_processed.channels() == 1) { + cv::cvtColor(display_processed, display_processed, cv::COLOR_GRAY2BGR); + } + + // Add target information + { + std::lock_guard lock(target_mutex_); + if (target_tracking_) { + cv::circle(display_processed, target_position_, 20, + cv::Scalar(0, 255, 255), 2); + cv::putText(display_processed, "TRACKING", cv::Point(10, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); + } + } + + // Check for stale data + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast( + now - processed_image_timestamp_).count(); + + if (elapsed > 500) { + cv::putText(display_processed, "STALE", cv::Point(500, 30), + cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); + } + } + } + + if (!display_processed.empty()) { + cv::imshow("Processed Image", display_processed); + } + } + + // Handle keyboard input + int key = cv::waitKey(frame_delay_ms) & 0xFF; + if (key == 27) { // ESC key + display_running_ = false; + RCLCPP_INFO(logger_, "ESC pressed, stopping display"); + } else if (key == 's' || key == 'S') { + save_current_frame(); + } + + frame_counter++; + + // Log FPS every 60 frames + if (frame_counter % 60 == 0) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast( + now - last_fps_time).count(); + double fps = 60000.0 / static_cast(std::max(1L, elapsed)); + RCLCPP_DEBUG(logger_, "Display FPS: %.1f", fps); + last_fps_time = now; + } + + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Display error: %s", e.what()); + } + } + + RCLCPP_INFO(logger_, "Display loop ended"); + } + + void save_current_frame() { + std::lock_guard lock(processed_mutex_); + if (!processed_image_.empty()) { + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + char time_str[100]; + std::strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", std::localtime(&time_t)); + + std::string filename = save_directory_ + "/frame_" + time_str + ".jpg"; + cv::imwrite(filename, processed_image_); + RCLCPP_INFO(logger_, "Saved frame: %s", filename.c_str()); + } + } + +private: + rclcpp::Logger logger_; + + // Topic names + std::string raw_image_topic_; + std::string processed_image_topic_; + std::string target_topic_; + + // ROS2 subscribers + rclcpp::Subscription::SharedPtr raw_image_sub_; + rclcpp::Subscription::SharedPtr processed_image_sub_; + rclcpp::Subscription::SharedPtr target_sub_; + + // Image data + cv::Mat raw_image_; + cv::Mat processed_image_; + std::chrono::steady_clock::time_point raw_image_timestamp_; + std::chrono::steady_clock::time_point processed_image_timestamp_; + + // Target data + cv::Point target_position_; + bool target_tracking_ = false; + std::chrono::steady_clock::time_point last_target_time_; + + // Mutexes for thread safety + std::mutex raw_mutex_; + std::mutex processed_mutex_; + std::mutex target_mutex_; + + // Display thread + std::thread display_thread_; + std::atomic display_running_{true}; + std::atomic display_available_{true}; + std::atomic initialized_{false}; + + // Configuration parameters + bool display_raw_ = false; + bool display_processed_ = true; + int max_fps_ = 30; + double window_scale_ = 1.0; + bool save_on_error_ = false; + std::string save_directory_ = "./debug_saved"; +}; + +} // namespace rmcs_dart_guidance + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::DebugDisplayComponent, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp new file mode 100644 index 00000000..0a901b40 --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp @@ -0,0 +1,498 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class CameraCalibrationComponent + : public rmcs_executor::Component + , public rclcpp::Node { +public: + CameraCalibrationComponent() + : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + image_topic_ = get_parameter("image_topic").as_string(); + board_width_ = static_cast(get_parameter("board_width").as_int()); + board_height_ = static_cast(get_parameter("board_height").as_int()); + square_size_ = get_parameter("square_size").as_double(); + save_path_ = get_parameter("save_path").as_string(); + enabled_ = get_parameter("enabled").as_bool(); + + RCLCPP_INFO(logger_, "CameraCalibrationComponent: enabled=%d, save_path='%s'", enabled_.load(), save_path_.c_str()); + + if (!enabled_) { + RCLCPP_INFO(logger_, "Camera calibration disabled by config."); + return; + } + + if (board_width_ < 2 || board_height_ < 2) { + RCLCPP_ERROR(logger_, "Invalid board size: (%d, %d)", board_width_, board_height_); + enabled_ = false; + return; + } + + std::string final_path = save_path_; + try { + std::filesystem::create_directories(final_path); + RCLCPP_INFO(logger_, "Save directory created/verified: %s", final_path.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Failed to create save directory %s: %s", final_path.c_str(), e.what()); + enabled_ = false; + return; + } + + std::string succ_dir = final_path + "/success"; + try { + std::filesystem::create_directories(succ_dir); + RCLCPP_INFO(logger_, "Created success subdirectory"); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Failed to create success subdirectory: %s", e.what()); + enabled_ = false; + return; + } + + for (int i = 0; i < board_height_; ++i) { + for (int j = 0; j < board_width_; ++j) { + object_points_.emplace_back(j * square_size_, i * square_size_, 0.0f); + } + } + + pattern_size_ = cv::Size(board_width_, board_height_); + + RCLCPP_INFO(logger_, "CameraCalibrationComponent ready with board=%dx%d, square=%.3fm, pattern size=%dx%d", + board_width_, board_height_, square_size_, pattern_size_.width, pattern_size_.height); + RCLCPP_INFO(logger_, "Absolute save path: %s", std::filesystem::absolute(final_path).c_str()); + } + + ~CameraCalibrationComponent() { + stop_calibration(); + } + + void update() override { + if (!enabled_ || initialized_) return; + + image_sub_ = this->create_subscription( + image_topic_, 10, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + image_callback(msg); + }); + + RCLCPP_INFO(logger_, "Subscribed to image topic: %s", image_topic_.c_str()); + + calibration_running_ = true; + calibration_thread_ = std::thread(&CameraCalibrationComponent::calibration_loop, this); + initialized_ = true; + + RCLCPP_INFO(logger_, "Calibration thread started (automatic mode)"); + } + +private: + void stop_calibration() { + calibration_running_ = false; + if (calibration_thread_.joinable()) { + calibration_thread_.join(); + } + } + + void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + std::lock_guard lock(img_mutex_); + try { + current_image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; + img_timestamp_ = std::chrono::steady_clock::now(); + RCLCPP_INFO(logger_, "Image received: %dx%d", current_image_.cols, current_image_.rows); + } catch (const cv_bridge::Exception& e) { + RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); + } + } + + void calibration_loop() { + const int target_fps = 30; + const std::chrono::milliseconds frame_delay(1000 / target_fps); + RCLCPP_INFO(logger_, "Calibration loop started."); + + cv::Mat last_gray; + int frame_counter = 0; + int empty_counter = 0; + + while (calibration_running_ && rclcpp::ok()) { + cv::Mat frame; + { + std::lock_guard lock(img_mutex_); + if (!current_image_.empty()) { + frame = current_image_.clone(); + } + } + + if (!frame.empty()) { + frame_counter++; + cv::Mat gray; + cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); + + cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + cv::Mat enhanced; + clahe->apply(gray, enhanced); + + std::vector corners; + bool found = false; + + found = cv::findChessboardCorners(enhanced, pattern_size_, corners, + cv::CALIB_CB_ADAPTIVE_THRESH + + cv::CALIB_CB_NORMALIZE_IMAGE + + cv::CALIB_CB_FAST_CHECK + + cv::CALIB_CB_EXHAUSTIVE); + + if (!found) { + found = cv::findChessboardCorners(gray, pattern_size_, corners, + cv::CALIB_CB_ADAPTIVE_THRESH + + cv::CALIB_CB_NORMALIZE_IMAGE + + cv::CALIB_CB_FAST_CHECK); + } + + if (found) { + cv::cornerSubPix(gray, corners, cv::Size(11,11), cv::Size(-1,-1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.001)); + + bool is_new = true; + if (!last_gray.empty()) { + cv::Mat diff; + cv::absdiff(gray, last_gray, diff); + double mean_diff = cv::mean(diff)[0]; + if (mean_diff < 5.0) { + is_new = false; + } + } + + if (is_new) { + last_gray = gray.clone(); + { + std::lock_guard lock(capture_mutex_); + captured_image_points_.push_back(corners); + captured_object_points_.push_back(object_points_); + RCLCPP_INFO(logger_, "Captured frame #%zu", captured_image_points_.size()); + } + + cv::Mat display = frame.clone(); + cv::drawChessboardCorners(display, pattern_size_, corners, true); + std::string filename = save_path_ + "/success/frame_" + std::to_string(frame_counter) + ".jpg"; + if (cv::imwrite(filename, display)) { + RCLCPP_INFO(logger_, "Saved success image: %s", filename.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to save success image: %s", filename.c_str()); + } + } + } + } else { + empty_counter++; + if (empty_counter % 30 == 0) { + RCLCPP_WARN(logger_, "No image received for %d frames", empty_counter); + } + } + + bool need_calibration = false; + + { + std::lock_guard lock(capture_mutex_); + if (static_cast(captured_image_points_.size()) >= min_capture_frames_) { + RCLCPP_INFO(logger_, "Collected %zu frames, stopping image reception and starting calibration...", + captured_image_points_.size()); + + image_sub_.reset(); + RCLCPP_INFO(logger_, "Image subscription cancelled."); + need_calibration = true; + } + } + + if (need_calibration) { + run_calibration(); + calibration_running_ = false; + break; + } + + std::this_thread::sleep_for(frame_delay); + } + RCLCPP_INFO(logger_, "Calibration loop ended."); + } + + void run_calibration() { + std::lock_guard lock(capture_mutex_); + + RCLCPP_INFO(logger_, "run_calibration entered with %zu frames", captured_image_points_.size()); + + if (captured_image_points_.size() < 5) { + RCLCPP_WARN(logger_, "Not enough frames captured (%zu). Need at least 5.", captured_image_points_.size()); + return; + } + + RCLCPP_INFO(logger_, "Running calibration with %zu frames...", captured_image_points_.size()); + + cv::Mat camera_matrix = cv::Mat::eye(3, 3, CV_64F); + cv::Mat dist_coeffs; + std::vector rvecs, tvecs; + double rms = -1.0; + + cv::Size image_size; + { + std::lock_guard lock(img_mutex_); + if (!current_image_.empty()) { + image_size = current_image_.size(); + RCLCPP_INFO(logger_, "Image size for calibration: %dx%d", image_size.width, image_size.height); + } else { + RCLCPP_ERROR(logger_, "No image available to determine size."); + return; + } + } + + RCLCPP_INFO(logger_, "Calling cv::calibrateCamera..."); + try { + rms = cv::calibrateCamera( + captured_object_points_, + captured_image_points_, + image_size, + camera_matrix, + dist_coeffs, + rvecs, + tvecs + ); + RCLCPP_INFO(logger_, "calibrateCamera completed with RMS = %.4f", rms); + } catch (const cv::Exception& e) { + RCLCPP_ERROR(logger_, "Calibration failed with OpenCV exception: %s", e.what()); + return; + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Calibration failed with standard exception: %s", e.what()); + return; + } + + if (rms < 0 || rms > 10.0) { + RCLCPP_WARN(logger_, "Calibration RMS (%.4f) seems too high, results may be unreliable.", rms); + } + double fx = camera_matrix.at(0,0); + double fy = camera_matrix.at(1,1); + if (fx <= 0 || fy <= 0) { + RCLCPP_ERROR(logger_, "Invalid focal length (fx=%.4f, fy=%.4f). Calibration failed.", fx, fy); + return; + } + + RCLCPP_INFO(logger_, "Calibration RMS: %.4f pixels", rms); + RCLCPP_INFO(logger_, "Camera matrix:\n%s", camera_matrixToString(camera_matrix).c_str()); + RCLCPP_INFO(logger_, "Distortion coefficients: %s", dist_coeffsToString(dist_coeffs).c_str()); + + saveCalibrationResults(camera_matrix, dist_coeffs, image_size, rms); + } + + static std::string camera_matrixToString(const cv::Mat& K) { + std::ostringstream oss; + oss << std::fixed << std::setprecision(4); + oss << "[ " << K.at(0,0) << ", " << K.at(0,1) << ", " << K.at(0,2) << ";\n"; + oss << " " << K.at(1,0) << ", " << K.at(1,1) << ", " << K.at(1,2) << ";\n"; + oss << " " << K.at(2,0) << ", " << K.at(2,1) << ", " << K.at(2,2) << " ]"; + return oss.str(); + } + + static std::string dist_coeffsToString(const cv::Mat& D) { + std::ostringstream oss; + oss << std::fixed << std::setprecision(6); + oss << "[ "; + for (int i = 0; i < static_cast(D.total()); ++i) { + if (i > 0) oss << ", "; + oss << D.at(i); + } + oss << " ]"; + return oss.str(); + } + + void saveCalibrationResults(const cv::Mat& camera_matrix, const cv::Mat& dist_coeffs, + const cv::Size& image_size, double rms) { + RCLCPP_INFO(logger_, "saveCalibrationResults started"); + + try { + std::filesystem::create_directories(save_path_); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Cannot create directory %s: %s", save_path_.c_str(), e.what()); + return; + } + + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf; + localtime_r(&time_t, &tm_buf); + + // 1. Save as YAML + { + std::ostringstream oss; + oss << save_path_ << "/calibration_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".yaml"; + std::string filename = oss.str(); + std::string abs_path = std::filesystem::absolute(filename).string(); + RCLCPP_INFO(logger_, "Saving YAML to: %s", abs_path.c_str()); + + cv::FileStorage fs(filename, cv::FileStorage::WRITE); + if (!fs.isOpened()) { + RCLCPP_ERROR(logger_, "Failed to open YAML file for writing: %s", filename.c_str()); + } else { + fs << "camera_matrix" << camera_matrix; + fs << "distortion_coefficients" << dist_coeffs; + fs << "image_width" << image_size.width; + fs << "image_height" << image_size.height; + fs << "rms" << rms; + fs << "board_width" << board_width_; + fs << "board_height" << board_height_; + fs << "square_size" << square_size_; + fs.release(); + if (std::filesystem::exists(filename)) { + RCLCPP_INFO(logger_, "YAML saved successfully"); + } else { + RCLCPP_ERROR(logger_, "YAML file not found after save"); + } + } + } + + // 2. Save as C++ header file + { + std::ostringstream oss; + oss << save_path_ << "/calibration_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".hpp"; + std::string filename = oss.str(); + std::ofstream ofs(filename); + if (ofs) { + ofs << "#pragma once\n\n"; + ofs << "namespace calibration_result {\n\n"; + ofs << "constexpr double camera_fx = " << camera_matrix.at(0,0) << ";\n"; + ofs << "constexpr double camera_fy = " << camera_matrix.at(1,1) << ";\n"; + ofs << "constexpr double camera_cx = " << camera_matrix.at(0,2) << ";\n"; + ofs << "constexpr double camera_cy = " << camera_matrix.at(1,2) << ";\n\n"; + ofs << "constexpr double distortion_k1 = " << dist_coeffs.at(0) << ";\n"; + ofs << "constexpr double distortion_k2 = " << dist_coeffs.at(1) << ";\n"; + ofs << "constexpr double distortion_p1 = " << dist_coeffs.at(2) << ";\n"; + ofs << "constexpr double distortion_p2 = " << dist_coeffs.at(3) << ";\n"; + if (dist_coeffs.total() > 4) + ofs << "constexpr double distortion_k3 = " << dist_coeffs.at(4) << ";\n"; + ofs << "\n} // namespace calibration_result\n"; + ofs.close(); + RCLCPP_INFO(logger_, "C++ header saved: %s", filename.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to save C++ header: %s", filename.c_str()); + } + } + + // 3. Save as binary (OpenCV bin) + { + std::ostringstream oss; + oss << save_path_ << "/calibration_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".bin"; + std::string filename = oss.str(); + cv::FileStorage fs(filename, cv::FileStorage::WRITE + cv::FileStorage::FORMAT_XML); + if (!fs.isOpened()) { + RCLCPP_ERROR(logger_, "Failed to open binary file for writing: %s", filename.c_str()); + } else { + fs << "camera_matrix" << camera_matrix; + fs << "distortion_coefficients" << dist_coeffs; + fs << "image_width" << image_size.width; + fs << "image_height" << image_size.height; + fs << "rms" << rms; + fs << "board_width" << board_width_; + fs << "board_height" << board_height_; + fs << "square_size" << square_size_; + fs.release(); + RCLCPP_INFO(logger_, "Binary file saved: %s", filename.c_str()); + } + } + + // 4. Save as Markdown + { + std::ostringstream oss; + oss << save_path_ << "/calibration_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".md"; + std::string filename = oss.str(); + std::ofstream ofs(filename); + if (ofs) { + ofs << "# Camera Calibration Result\n\n"; + ofs << "Date: " << std::put_time(&tm_buf, "%Y-%m-%d %H:%M:%S") << "\n\n"; + ofs << "## Board Parameters\n"; + ofs << "- Board size: " << board_width_ << " x " << board_height_ << " inner corners\n"; + ofs << "- Square size: " << square_size_ << " m\n\n"; + ofs << "## Camera Matrix (fx, fy, cx, cy)\n"; + ofs << "| fx | fy | cx | cy |\n"; + ofs << "|---|---|---|---|\n"; + ofs << "| " << camera_matrix.at(0,0) << " | " + << camera_matrix.at(1,1) << " | " + << camera_matrix.at(0,2) << " | " + << camera_matrix.at(1,2) << " |\n\n"; + ofs << "## Distortion Coefficients\n"; + ofs << "| k1 | k2 | p1 | p2 | k3 |\n"; + ofs << "|---|---|---|---|---|\n"; + ofs << "| " << dist_coeffs.at(0) << " | " + << dist_coeffs.at(1) << " | " + << dist_coeffs.at(2) << " | " + << dist_coeffs.at(3) << " | "; + if (dist_coeffs.total() > 4) + ofs << dist_coeffs.at(4); + else + ofs << "0.0"; + ofs << " |\n\n"; + ofs << "## Reprojection Error (RMS)\n"; + ofs << "- RMS = " << rms << " pixels\n"; + ofs.close(); + RCLCPP_INFO(logger_, "Markdown file saved: %s", filename.c_str()); + } else { + RCLCPP_ERROR(logger_, "Failed to save Markdown file: %s", filename.c_str()); + } + } + + RCLCPP_INFO(logger_, "All calibration result files saved to %s", save_path_.c_str()); + RCLCPP_INFO(logger_, "\nParameters for DartVisionCore:\n" + "camera_focal_length_x: %.4f\n" + "camera_focal_length_y: %.4f\n" + "camera_principal_point_x: %.4f\n" + "camera_principal_point_y: %.4f\n", + camera_matrix.at(0,0), + camera_matrix.at(1,1), + camera_matrix.at(0,2), + camera_matrix.at(1,2)); + } + +private: + rclcpp::Logger logger_; + + rclcpp::Subscription::SharedPtr image_sub_; + std::string image_topic_; + + int board_width_; + int board_height_; + double square_size_; + cv::Size pattern_size_; + std::vector object_points_; + std::vector> captured_object_points_; + std::vector> captured_image_points_; + const int min_capture_frames_ = 10; + + std::string save_path_; + + cv::Mat current_image_; + std::chrono::steady_clock::time_point img_timestamp_; + std::mutex img_mutex_; + std::mutex capture_mutex_; + + std::thread calibration_thread_; + std::atomic calibration_running_{false}; + std::atomic enabled_{false}; + std::atomic initialized_{false}; +}; + +} // namespace rmcs_dart_guidance + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::CameraCalibrationComponent, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp new file mode 100644 index 00000000..543699fb --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class ImagePublisher + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ImagePublisher() + : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + register_input(get_parameter("Interface_name").as_string(), input_image_); + + image_publisher_ = + this->create_publisher(get_parameter("topic_name").as_string(), 1000); + + publish_freq_ = get_parameter("publish_freq").as_double(); + publish_freq_ = MAX(0, MIN(publish_freq_, 1000)); + image_type_ = get_parameter("image_type").as_string(); + timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_freq_)), [this]() { + sensor_msgs::msg::Image publish_image_msg; + cv_bridge::CvImage cv_image(std_msgs::msg::Header(), image_type_, *input_image_); + publish_image_msg = *cv_image.toImageMsg(); + image_publisher_->publish(publish_image_msg); + }); + } + + void update() override {} + +private: + rclcpp::Logger logger_; + + double publish_freq_ = 60.0; // Hz + std::string image_type_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr image_publisher_; + InputInterface input_image_; +}; +} // namespace rmcs_dart_guide + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::ImagePublisher, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp new file mode 100644 index 00000000..7e4cce15 --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp @@ -0,0 +1,118 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_dart_guidance { + +class TestImageSaver : public rmcs_executor::Component, public rclcpp::Node { +public: + TestImageSaver() + : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , logger_(get_logger()) { + + // 读取测试参数 + std::string save_dir = "./test_images"; + if (has_parameter("save_directory")) { + save_dir = get_parameter("save_directory").as_string(); + } + + int test_count = 5; + if (has_parameter("test_count")) { + test_count = static_cast(get_parameter("test_count").as_int()); + } + + int interval_ms = 1000; + if (has_parameter("interval_ms")) { + interval_ms = static_cast(get_parameter("interval_ms").as_int()); + } + + RCLCPP_INFO(logger_, "Starting image saver test:"); + RCLCPP_INFO(logger_, " Directory: %s", save_dir.c_str()); + RCLCPP_INFO(logger_, " Count: %d", test_count); + RCLCPP_INFO(logger_, " Interval: %d ms", interval_ms); + + // 创建目录 + try { + std::filesystem::create_directories(save_dir); + RCLCPP_INFO(logger_, "Created directory: %s", save_dir.c_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Failed to create directory: %s", e.what()); + return; + } + + // 生成测试图像并保存 + for (int i = 0; i < test_count; ++i) { + test_save_image(save_dir, i); + std::this_thread::sleep_for(std::chrono::milliseconds(interval_ms)); + } + + RCLCPP_INFO(logger_, "Test completed. Check directory: %s", save_dir.c_str()); + } + + void update() override { + // 测试完成后停止更新 + } + +private: + void test_save_image(const std::string& directory, int index) { + // 创建测试图像 + cv::Mat test_image(480, 640, CV_8UC3, cv::Scalar(0, 0, 0)); + + // 添加一些内容 + cv::rectangle(test_image, cv::Point(100, 100), cv::Point(540, 380), cv::Scalar(0, 255, 0), 2); + cv::circle(test_image, cv::Point(320, 240), 50, cv::Scalar(255, 0, 0), -1); + cv::putText(test_image, "Test " + std::to_string(index), cv::Point(200, 400), + cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2); + + // 生成文件名 + auto now = std::chrono::system_clock::now(); + auto time_t = std::chrono::system_clock::to_time_t(now); + std::tm tm_buf; + localtime_r(&time_t, &tm_buf); + + std::ostringstream oss; + oss << directory << "/test_" + << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") + << "_" << std::setw(3) << std::setfill('0') << index + << ".png"; + + std::string filename = oss.str(); + + // 保存图像 + try { + bool success = cv::imwrite(filename, test_image); + if (success) { + RCLCPP_INFO(logger_, "Saved test image: %s", filename.c_str()); + + // 验证文件存在 + if (std::filesystem::exists(filename)) { + auto file_size = std::filesystem::file_size(filename); + RCLCPP_INFO(logger_, " File size: %ld bytes", file_size); + } else { + RCLCPP_ERROR(logger_, " File not found after saving!"); + } + } else { + RCLCPP_ERROR(logger_, "Failed to save image: %s", filename.c_str()); + } + } catch (const cv::Exception& e) { + RCLCPP_ERROR(logger_, "OpenCV exception: %s", e.what()); + } catch (const std::exception& e) { + RCLCPP_ERROR(logger_, "Exception: %s", e.what()); + } + } + + rclcpp::Logger logger_; +}; + +} // namespace rmcs_dart_guidance + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::TestImageSaver, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp index 9ce532b9..bebe7a91 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp @@ -17,4 +17,21 @@ enum class DartLaunchStages { 特殊： READY -> INIT：退膛，转动带下去接滑台使其复位，避免空放 */ +enum class DartFillingStages { + INIT, + DOWNING, + READY, + EMPTY, + LIFTING, + FILLING, +}; +/* +说明： + INIT：初始状态，已经有一发镖在升降平台上，也代表着升降平台在up位置且有镖的状态 + DROWING：升降平台下降 + READY：下降到位，镖被推上滑台，可以发射，双READY发射 + EMPTY：表示升降平台无镖，调试用，不参与控制 + LIFTING：升降平台上升 + FILLING：正在填装中，填装完成进入INIT +*/ } // namespace rmcs_msgs \ No newline at end of file From c07447317582d67a29b368f5e4c3bac866c3eb2d Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Sun, 8 Mar 2026 19:53:48 +0800 Subject: [PATCH 30/45] removed rmcs_dart_guidance,will submit as submodule --- rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt | 97 ---- rmcs_ws/src/rmcs_dart_guidance/package.xml | 32 -- rmcs_ws/src/rmcs_dart_guidance/plugins.xml | 21 - .../src/vision/angle_solver.hpp | 142 ----- .../src/vision/dart_vision_core.cpp | 341 ------------ .../src/vision/identifier.hpp | 191 ------- .../rmcs_dart_guidance/src/vision/tracker.hpp | 112 ---- .../test/debug_image_display.cpp | 321 ----------- .../test/get_camera_parameter.cpp | 498 ------------------ .../test/image_publisher.cpp | 58 -- .../test/image_saving_test.cpp | 118 ----- 11 files changed, 1931 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/package.xml delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/plugins.xml delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp delete mode 100644 rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp diff --git a/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt b/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt deleted file mode 100644 index a2b3f6a1..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/CMakeLists.txt +++ /dev/null @@ -1,97 +0,0 @@ -cmake_minimum_required(VERSION 3.12) -project(rmcs_dart_guidance) - -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_BUILD_TYPE "Release") -add_compile_options(-Wall -Wextra -Wpedantic -O3) - -find_package(ament_cmake REQUIRED) - -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(type_description_interfaces REQUIRED) - -find_package(OpenCV 4.5 REQUIRED) -find_package(OpenVINO REQUIRED) -find_package(Ceres REQUIRED) - -find_package(rmcs_executor REQUIRED) -find_package(rmcs_description REQUIRED) -find_package(fast_tf REQUIRED) -find_package(hikcamera REQUIRED) - -include_directories( - ${PROJECT_SOURCE_DIR}/src/ - ${EIGEN3_INCLUDE_DIRS} - ${type_description_interfaces_INCLUDE_DIRS} - ${rclcpp_INCLUDE_DIRS} - ${rmcs_executor_INCLUDE_DIRS} - ${rmcs_description_INCLUDE_DIRS} - ${fast_tf_INCLUDE_DIRS} - ${hikcamera_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${OpenVINO_INCLUDE_DIRS} - ${Ceres_INCLUDE_DIRS} -) - -file(GLOB_RECURSE PROJECT_SOURCE - CONFIGURE_DEPENDS - ${PROJECT_SOURCE_DIR}/src/*.cpp - ${PROJECT_SOURCE_DIR}/src/*.cc - ${PROJECT_SOURCE_DIR}/test/*.cpp - ${PROJECT_SOURCE_DIR}/test/*.cc -) - -add_library( - ${PROJECT_NAME} SHARED - ${PROJECT_SOURCE} -) - -ament_target_dependencies(${PROJECT_NAME} - rclcpp - std_msgs - sensor_msgs - cv_bridge - geometry_msgs - visualization_msgs -) - -target_link_libraries( - ${PROJECT_NAME} - ${rclcpp_LIBRARIES} - ${std_msgs_LIBRARIES} - ${sensor_msgs_LIBRARIES} - ${cv_bridge_LIBRARIES} - ${geometry_msgs_LIBRARIES} - ${visualization_msgs_LIBRARIES} - ${type_description_interfaces_LIBRARIES} - - ${OpenCV_LIBS} - ${OpenVINO_LIBRARIES} - ${Ceres_LIBRARIES} - ${rmcs_executor_LIBRARIES} - ${rmcs_description_LIBRARIES} - ${fast_tf_LIBRARIES} - ${hikcamera_LIBRARIES} -) - -install( - TARGETS - ${PROJECT_NAME} - DESTINATION - lib/ -) - -find_package(pluginlib REQUIRED) -pluginlib_export_plugin_description_file( - rmcs_executor plugins.xml -) - -ament_package() \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/package.xml b/rmcs_ws/src/rmcs_dart_guidance/package.xml deleted file mode 100644 index 474d59ab..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - rmcs_dart_guidance - 0.0.0 - TODO: Package description - ubuntu - TODO: License declaration - - ament_cmake - - rclcpp - std_msgs - pluginlib - hikcamera - rmcs_executor - rmcs_description - fast_tf - sensor_msgs - cv_bridge - geometry_msgs - tf2_ros - tf2_geometry_msgs - eigen3_cmake_module - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/rmcs_ws/src/rmcs_dart_guidance/plugins.xml b/rmcs_ws/src/rmcs_dart_guidance/plugins.xml deleted file mode 100644 index 4f1ad0f0..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/plugins.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - Image processing node. - - - test node. - - - test node. - - - test node. - - - test node. - - - \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp deleted file mode 100644 index 32de6fbe..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/src/vision/angle_solver.hpp +++ /dev/null @@ -1,142 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class DartGuidanceAngleSolver { -public: - DartGuidanceAngleSolver() { Init(); } - - void Init() { - focal_length_x_ = 800.0; - focal_length_y_ = 800.0; - principal_point_x_ = 320.0; - principal_point_y_ = 240.0; - target_real_area_ = 0.0; - is_intrinsics_set_ = false; - is_extrinsic_set_ = false; - cached_target_area_ = 0.0; - cache_valid_ = false; - } - - void set_default(double flx, double fly, double ppx, double ppy, double real_area) { - focal_length_x_ = flx; - focal_length_y_ = fly; - principal_point_x_ = ppx; - principal_point_y_ = ppy; - target_real_area_ = real_area; - is_intrinsics_set_ = true; - } - - void set_extrinsic(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) { - R_camera2launcher_ = R; - t_camera2launcher_ = t; - is_extrinsic_set_ = true; - } - - void set_cached_target_area(double area) { - cached_target_area_ = area; - cache_valid_ = (area > 1e-8); - } - - double estimate_distance(double pixel_area) const { - if (pixel_area <= 1e-8 || target_real_area_ <= 0 || !is_intrinsics_set_) - return -1.0; - return std::sqrt(target_real_area_ * focal_length_x_ * focal_length_y_ / pixel_area); - } - - Eigen::Vector2d calculate_angles_error_simple(const cv::Point2i& pixel) const { - double dx = pixel.x - principal_point_x_; - double dy = pixel.y - principal_point_y_; - double yaw = std::atan2(dx, focal_length_x_); - double pitch = std::atan2(dy, focal_length_y_); - return {yaw, pitch}; - } - - Eigen::Vector2d compute_angles_error_accurate(const cv::Point2i& pixel, double distance) const { - if (!is_intrinsics_set_ || !is_extrinsic_set_ || distance <= 0) - return Eigen::Vector2d::Zero(); - - double Zc = distance; - double Xc = (pixel.x - principal_point_x_) * Zc / focal_length_x_; - double Yc = (pixel.y - principal_point_y_) * Zc / focal_length_y_; - Eigen::Vector3d P_camera(Xc, Yc, Zc); - Eigen::Vector3d P_launcher = R_camera2launcher_ * P_camera + t_camera2launcher_; - - double x = P_launcher.x(); - double y = P_launcher.y(); - double z = P_launcher.z(); - - double yaw = std::atan2(x, z); - double pitch = std::atan2(y, std::sqrt(x*x + y*y)); - return {yaw, pitch}; - } - - Eigen::Vector2d update(const cv::Point2i& pixel, double pixel_area, bool use_accurate = true) { - last_pixel_ = pixel; - last_area_ = pixel_area; - last_distance_ = estimate_distance(pixel_area); - - if (use_accurate && is_extrinsic_set_ && last_distance_ > 0) { - last_angle_error_ = compute_angles_error_accurate(pixel, last_distance_); - } else { - last_angle_error_ = calculate_angles_error_simple(pixel); - } - - last_process_time_ = std::chrono::steady_clock::now(); - return last_angle_error_; - } - - Eigen::Vector2d update(const cv::Point2i& pixel, bool use_accurate = true) { - last_pixel_ = pixel; - if (cache_valid_ && target_real_area_ > 0 && is_intrinsics_set_) { - last_distance_ = estimate_distance(cached_target_area_); - } else { - last_distance_ = -1.0; - } - - if (use_accurate && is_extrinsic_set_ && last_distance_ > 0) { - last_angle_error_ = compute_angles_error_accurate(pixel, last_distance_); - } else { - last_angle_error_ = calculate_angles_error_simple(pixel); - } - - last_process_time_ = std::chrono::steady_clock::now(); - return last_angle_error_; - } - - Eigen::Vector2d getLastAngleError() const { return last_angle_error_; } - double getLastDistance() const { return last_distance_; } - cv::Point2i getLastPixel() const { return last_pixel_; } - double getLastArea() const { return last_area_; } - bool isIntrinsicsSet() const { return is_intrinsics_set_; } - bool isExtrinsicSet() const { return is_extrinsic_set_; } - -private: - bool is_intrinsics_set_ = false; - bool is_extrinsic_set_ = false; - double focal_length_x_; - double focal_length_y_; - double principal_point_x_; - double principal_point_y_; - double target_real_area_; - - Eigen::Matrix3d R_camera2launcher_; - Eigen::Vector3d t_camera2launcher_; - - double cached_target_area_ = 0.0; - bool cache_valid_ = false; - - mutable cv::Point2i last_pixel_ = cv::Point2i(-1, -1); - mutable double last_area_ = 0.0; - mutable double last_distance_ = 0.0; - mutable Eigen::Vector2d last_angle_error_ = Eigen::Vector2d::Zero(); - mutable std::chrono::steady_clock::time_point last_process_time_; -}; - -} // namespace rmcs_dart_guidance \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp deleted file mode 100644 index 10980afe..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/src/vision/dart_vision_core.cpp +++ /dev/null @@ -1,341 +0,0 @@ -#include "identifier.hpp" -#include "tracker.hpp" -#include "angle_solver.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class DartVisionCore - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartVisionCore() - : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { - - camera_profile_.invert_image = get_parameter("invert_image").as_bool(); - camera_profile_.exposure_time = std::chrono::microseconds(get_parameter("exposure_time").as_int()); - camera_profile_.gain = static_cast(get_parameter("gain").as_double()); - - lower_limit_default_ = cv::Scalar(get_parameter("L_H").as_double(), get_parameter("L_S").as_double(), get_parameter("L_V").as_double()); - upper_limit_default_ = cv::Scalar(get_parameter("U_H").as_double(), get_parameter("U_S").as_double(), get_parameter("U_V").as_double()); - - enable_image_saving_ = has_parameter("enable_image_saving") ? - get_parameter("enable_image_saving").as_bool() : false; - save_directory_ = has_parameter("image_save_directory") ? - get_parameter("image_save_directory").as_string() : "./saved_images"; - save_interval_ms_ = has_parameter("image_save_interval_ms") ? - static_cast(get_parameter("image_save_interval_ms").as_int()) : 1000; - save_raw_image_ = has_parameter("save_raw_image") ? - get_parameter("save_raw_image").as_bool() : true; - save_processed_image_ = has_parameter("save_processed_image") ? - get_parameter("save_processed_image").as_bool() : false; - - double fx_ = get_parameter_or("camera_focal_length_x", 4242.6083); - double fy_ = get_parameter_or("camera_focal_length_y", 4227.9942); - double cx_ = get_parameter_or("camera_principal_point_x", 632.9298); - double cy_ = get_parameter_or("camera_principal_point_y", 556.6188); - - double target_diameter_ = get_parameter_or("target_diameter", 0.1); - double target_area_ = CV_PI * std::pow(target_diameter_ / 2.0, 2); - - std::vector R_vec = get_parameter_or("cam2launcher_R", - std::vector{1,0,0,0,1,0,0,0,1}); - std::vector T_vec = get_parameter_or("cam2launcher_T", - std::vector{0,0,0}); - Eigen::Matrix3d R = Eigen::Map>(R_vec.data()); - Eigen::Vector3d t = Eigen::Map(T_vec.data()); - angle_solver_.set_default(fx_, fy_, cx_, cy_, target_area_); - angle_solver_.set_extrinsic(R, t); - angle_solver_.set_cached_target_area(0.0); - - image_capture_ = std::make_unique(camera_profile_); - - identifier_.set_default_limit(lower_limit_default_, upper_limit_default_); - identifier_.Init(); - - if (enable_image_saving_) { - try { - std::filesystem::create_directories(save_directory_); - RCLCPP_INFO(logger_, "Created image save directory: %s", save_directory_.c_str()); - - test_write_permission(); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Failed to create save directory %s: %s", - save_directory_.c_str(), e.what()); - enable_image_saving_ = false; - } - } - - register_output("/dart_guidance/camera/camera_image", camera_image_); - register_output("/dart_guidance/camera/display_image", display_image_); - register_output("/dart_guidance/camera/target_position", target_position_, PointT(-1, -1)); - register_output("/dart_guidance/tracker/tracking", tracking_); - register_output("/dart_guidance/angle/error", angle_error_); - if (enable_image_saving_) { - RCLCPP_INFO(logger_, "Image saving enabled:"); - RCLCPP_INFO(logger_, " Directory: %s", save_directory_.c_str()); - RCLCPP_INFO(logger_, " Interval: %d ms", save_interval_ms_); - RCLCPP_INFO(logger_, " Save raw: %s", save_raw_image_ ? "true" : "false"); - RCLCPP_INFO(logger_, " Save processed: %s", save_processed_image_ ? "true" : "false"); - } else { - RCLCPP_INFO(logger_, "Image saving disabled"); - } - - camera_thread_ = std::thread(&DartVisionCore::camera_update, this); - update_time_point_ = std::chrono::steady_clock::now(); - } - - ~DartVisionCore() { - if (camera_thread_.joinable()) { - camera_thread_.join(); - } - } - - void update() override {} - - std::chrono::steady_clock::time_point update_time_point_; - -private: - void camera_update() { - int frame_counter = 0; - int saved_raw_counter = 0; - int saved_processed_counter = 0; - - std::chrono::steady_clock::time_point last_save_time = std::chrono::steady_clock::now(); - - while (rclcpp::ok()) { - try { - frame_counter++; - - cv::Mat raw_image = image_capture_->read(); - if (raw_image.empty()) { - RCLCPP_WARN_THROTTLE(logger_, *this->get_clock(), 1000, - "Received empty image from camera"); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - continue; - } - - cv::line(raw_image, cv::Point(645, 0), cv::Point(645, 720), cv::Scalar(255, 0, 255), 1); - - *camera_image_ = raw_image; - - auto now = std::chrono::steady_clock::now(); - auto elapsed_ms = std::chrono::duration_cast( - now - last_save_time).count(); - - if (enable_image_saving_ && save_raw_image_ && elapsed_ms >= save_interval_ms_) { - if (save_image(raw_image, "raw")) { - saved_raw_counter++; - RCLCPP_INFO(logger_, "Saved raw image %d", saved_raw_counter); - last_save_time = now; - } - } - - cv::Mat preprocessed_image; - image_to_binary(raw_image, preprocessed_image); - - cv::Mat display_image = preprocessed_image.clone(); - process_frame(preprocessed_image, display_image); - - cv::line(display_image, cv::Point(0, 645), cv::Point(720, 645), cv::Scalar(255, 0, 255), 1); - *display_image_ = display_image; - - if (enable_image_saving_ && save_processed_image_ && elapsed_ms >= save_interval_ms_) { - if (save_image(display_image, "processed")) { - saved_processed_counter++; - RCLCPP_INFO(logger_, "Saved processed image %d", saved_processed_counter); - } - } - - if (frame_counter % 30 == 0) { - RCLCPP_DEBUG(logger_, "Processed %d frames", frame_counter); - } - - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Error in camera_update: %s", e.what()); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - } - - RCLCPP_INFO(logger_, "Camera thread stopped. Frames: %d, Raw saved: %d, Processed saved: %d", - frame_counter, saved_raw_counter, saved_processed_counter); - } - - bool save_image(const cv::Mat& image, const std::string& type) { - try { - auto now = std::chrono::system_clock::now(); - auto time_t = std::chrono::system_clock::to_time_t(now); - std::tm tm_buf; - localtime_r(&time_t, &tm_buf); - - std::ostringstream oss; - oss << save_directory_ << "/" << type - << "_" << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") - << "_" << std::setw(4) << std::setfill('0') << save_counter_++ - << ".jpg"; - - std::string filename = oss.str(); - - bool success = cv::imwrite(filename, image); - if (success) { - if (std::filesystem::exists(filename)) { - return true; - } else { - RCLCPP_ERROR(logger_, "File not created: %s", filename.c_str()); - } - } else { - RCLCPP_ERROR(logger_, "cv::imwrite failed for: %s", filename.c_str()); - } - } catch (const cv::Exception& e) { - RCLCPP_ERROR(logger_, "OpenCV error saving image: %s", e.what()); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Error saving image: %s", e.what()); - } - - return false; - } - - void test_write_permission() { - std::string test_file = save_directory_ + "/test_write.jpg"; - cv::Mat test_image(100, 100, CV_8UC3, cv::Scalar(255, 0, 0)); - - try { - bool success = cv::imwrite(test_file, test_image); - if (success && std::filesystem::exists(test_file)) { - auto file_size = std::filesystem::file_size(test_file); - RCLCPP_INFO(logger_, "Write test successful: %s (%ld bytes)", test_file.c_str(), file_size); - - std::filesystem::remove(test_file); - } else { - RCLCPP_ERROR(logger_, "Write test failed: %s", test_file.c_str()); - } - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Write test exception: %s", e.what()); - } - } - - void process_frame(cv::Mat& preprocessed_image, cv::Mat& display_image) { - if (!is_tracker_stage_) { - identifier_.update(preprocessed_image); - - cv::putText( - display_image, "Identifying", cv::Point(10, 30), - cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 255), 2); - - if (!identifier_.result_status_()) { - *target_position_ = PointT(-1, -1); - *tracking_ = false; - *angle_error_ = Eigen::Vector2d::Zero(); - } else { - cv::Point2i initial_position = identifier_.get_result(); - RCLCPP_INFO(logger_, "Target initial position:(%d,%d)", initial_position.x, initial_position.y); - - double init_area = identifier_.get_target_area(); - angle_solver_.set_cached_target_area(init_area); - is_tracker_stage_ = true; - tracker_.Init(initial_position); - - *target_position_ = initial_position; - *tracking_ = true; - *angle_error_ = angle_solver_.update(initial_position, true); - } - } else { - tracker_.update(preprocessed_image); - cv::Point2i current_position = tracker_.get_current_position(); - - bool is_tracking = tracker_.get_tracking_status(); - cv::putText( - display_image, is_tracking ? "Tracking" : "Lost", cv::Point(10, 30), - cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 0, 255), 2); - - if (!is_tracking) { - tracker_loss_count_++; - *target_position_ = PointT(-1, -1); - *tracking_ = false; - *angle_error_ = Eigen::Vector2d::Zero(); - - if (tracker_loss_count_ > 100) { - is_tracker_stage_ = false; - identifier_.Init(); - angle_solver_.set_cached_target_area(0.0); - tracker_loss_count_ = 0; - RCLCPP_INFO(logger_, "Target lost, switching to identification mode"); - } - } else { - tracker_loss_count_ = 0; - *target_position_ = current_position; - *tracking_ = true; - *angle_error_ = angle_solver_.update(current_position, true); - cv::circle(display_image, current_position, 20, cv::Scalar(255, 0, 255), 2); - } - } - } - - void image_to_binary(const cv::Mat& src, cv::Mat& output) { - cv::Mat HSV_image; - cv::cvtColor(src, HSV_image, cv::COLOR_BGR2HSV); - - cv::Mat binary; - cv::inRange(HSV_image, lower_limit_default_, upper_limit_default_, binary); - - static cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(9, 9)); - cv::morphologyEx(binary, binary, cv::MORPH_OPEN, kernel); - cv::morphologyEx(binary, binary, cv::MORPH_CLOSE, kernel); - output = binary; - } - - rclcpp::Logger logger_; - - bool enable_image_saving_ = false; - std::string save_directory_ = "./saved_images"; - int save_interval_ms_ = 1000; - bool save_raw_image_ = true; - bool save_processed_image_ = false; - int save_counter_ = 0; - - std::thread camera_thread_; - std::mutex camera_thread_mtx_; - - cv::Scalar lower_limit_default_, upper_limit_default_; - - hikcamera::ImageCapturer::CameraProfile camera_profile_; - std::unique_ptr image_capture_; - - OutputInterface camera_image_; - OutputInterface display_image_; - OutputInterface tracking_; - OutputInterface target_position_; - OutputInterface angle_error_; - - DartGuidanceIdentifier identifier_; - DartGuidanceTracker tracker_; - DartGuidanceAngleSolver angle_solver_; - bool is_tracker_stage_ = false; - int tracker_loss_count_ = 0; -}; -} // namespace rmcs_dart_guidance - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::DartVisionCore, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp deleted file mode 100644 index 64f2b284..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/src/vision/identifier.hpp +++ /dev/null @@ -1,191 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace rmcs_dart_guidance { - -using PointT = cv::Point2i; - -struct TargetData { - PointT first_position; - PointT latest_position; - double max_move_distance; - int catch_count; - int miss_count; - double area; -}; - -class DartGuidanceIdentifier { -public: - DartGuidanceIdentifier() { - Init(); - } - - void Init() { - detect_frame_count_ = 0; - result_ready_ = false; - possible_targets_collection_.clear(); - start_time_ = std::chrono::steady_clock::now(); - last_target_area_ = 0.0; - } - - void set_default_limit(const cv::Scalar& lower, const cv::Scalar& upper) { - lower_limit_ = lower; - upper_limit_ = upper; - } - - void update(const cv::Mat& binary_image) { - if (result_ready_) { - Init(); - } - - std::vector> possible_targets = process(binary_image); - target_filter(possible_targets); - - detect_frame_count_++; - - if (detect_frame_count_ < 80) { - return; - } - - double highest_score = 0; - size_t most_possible_target_id = -1; - - if (possible_targets_collection_.empty()) { - Init(); - } else { - for (size_t i = 0; i < possible_targets_collection_.size(); i++) { - double this_score = possible_targets_collection_[i].catch_count - + 1000 / (possible_targets_collection_[i].max_move_distance + 1); - - if (this_score > highest_score) { - most_possible_target_id = i; - highest_score = this_score; - } - } - target_initial_position_ = possible_targets_collection_[most_possible_target_id].latest_position; - last_target_area_ = possible_targets_collection_[most_possible_target_id].area; - result_ready_ = true; - - auto end_time_ = std::chrono::steady_clock::now(); - auto delta_time = std::chrono::duration_cast(end_time_ - start_time_).count(); - last_process_time_ms_ = delta_time; - } - } - - cv::Mat get_display_image() { return display_image_; } - bool result_status_() const { return result_ready_; } - PointT get_result() { return target_initial_position_; } - double get_target_area() const { return last_target_area_; } - - std::string get_debug_info() const { - std::string info = "Identifier: Frames=" + std::to_string(detect_frame_count_) + - ", Targets=" + std::to_string(possible_targets_collection_.size()) + - ", Result=" + (result_ready_ ? "Ready" : "Not Ready"); - if (result_ready_) { - info += ", Pos=(" + std::to_string(target_initial_position_.x) + - "," + std::to_string(target_initial_position_.y) + ")"; - } - if (last_process_time_ms_ > 0) { - info += ", Time=" + std::to_string(last_process_time_ms_) + "ms"; - } - return info; - } - -private: - - cv::Mat display_image_; - cv::Scalar lower_limit_, upper_limit_; - cv::Scalar latest_lower_limit_, latest_upper_limit_; - std::vector possible_targets_collection_; - int detect_frame_count_; - PointT target_initial_position_; - bool result_ready_; - - std::chrono::steady_clock::time_point start_time_; - int64_t last_process_time_ms_ = 0; - double last_target_area_ = 0.0; - - static std::vector> process(const cv::Mat& binary_image) { - std::vector> contours; - std::vector hierarchy; - cv::findContours(binary_image, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); - - std::vector> possible_targets; - for (const auto& contour : contours) { - double area = cv::contourArea(contour); - if (area < 64 || area > 5000) continue; - if (contour.size() < 5) continue; - - cv::Point2f circle_center; - float circle_radius; - cv::minEnclosingCircle(contour, circle_center, circle_radius); - double enclosing_circle_area = CV_PI * circle_radius * circle_radius; - double area_ratio = 0; - if (enclosing_circle_area > 0) { - area_ratio = area / enclosing_circle_area; - } - if (area_ratio >= 0.8) { - possible_targets.emplace_back(cv::Point2i(circle_center), area); - } - } - return possible_targets; - } - - void target_filter(const std::vector>& points) { - const int distance_threshold = 10; - std::vector matched(points.size(), false); - - for (auto& collected: possible_targets_collection_) { - double min_distance = std::numeric_limits::max(); - int point_id = -1; - - for (size_t i = 0; i < points.size(); ++i) { - if (matched[i]) continue; - - double distance = cv::norm(points[i].first - collected.latest_position); - if (distance < distance_threshold && distance < min_distance) { - min_distance = distance; - point_id = static_cast(i); - } - } - - if (point_id != -1) { - double this_move_distance = cv::norm(collected.first_position - points[point_id].first); - collected.max_move_distance = std::max(collected.max_move_distance, this_move_distance); - collected.latest_position = points[point_id].first; - collected.area = points[point_id].second; - collected.catch_count++; - collected.miss_count = 0; - matched[point_id] = true; - } else { - collected.miss_count++; - } - } - - for (size_t i = 0; i < points.size(); ++i) { - if (!matched[i]) { - TargetData new_target_data(points[i].first, points[i].first, 0, 1, 0, points[i].second); - possible_targets_collection_.emplace_back(new_target_data); - } - } - - possible_targets_collection_.erase( - std::remove_if( - possible_targets_collection_.begin(), - possible_targets_collection_.end(), - [](const TargetData& target) { return target.miss_count >= 20; } - ), - possible_targets_collection_.end() - ); - } -}; -} \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp b/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp deleted file mode 100644 index 9ce0948f..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/src/vision/tracker.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once - -#include -#include - -namespace rmcs_dart_guidance { -class DartGuidanceTracker{ -public: - explicit DartGuidanceTracker(double min_contour_area = 20.0, double max_distance_threshold = 200.0) - : current_position_(-1, -1) - , is_initialized_(false) - , min_contour_area_(min_contour_area) - , max_distance_threshold_(max_distance_threshold) {} - - void Init(cv::Point2i& initial_position) { - current_position_ = initial_position; - is_initialized_ = true; - tracking_count_ = 0; - lost_count_ = 0; - last_track_time_ = std::chrono::steady_clock::now(); - } - - void update(const cv::Mat& binary_image) { - auto start_time = std::chrono::steady_clock::now(); - - if (!is_initialized_) { - return; - } - - if (binary_image.empty() || binary_image.channels() != 1 || binary_image.type() != CV_8U) { - return; - } - - std::vector> contours; - cv::findContours(binary_image, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE); - - double min_distance = std::numeric_limits::max(); - cv::Point2i best_centroid = current_position_; - bool found_match = false; - - - for (const auto& contour : contours) { - double area = cv::contourArea(contour); - if (area < min_contour_area_) { - continue; - } - - cv::Moments moments = cv::moments(contour); - - if (moments.m00 == 0) { - continue; - } - - cv::Point2f centroid(static_cast(moments.m10 / moments.m00), static_cast(moments.m01 / moments.m00)); - - double distance = cv::norm(centroid - (cv::Point2f)current_position_); - - if (distance < min_distance) { - min_distance = distance; - best_centroid = centroid; - found_match = true; - } - - valid_contours_++; - } - - if (found_match && min_distance < max_distance_threshold_) { - current_position_ = cv::Point2i(static_cast(std::round(best_centroid.x)), static_cast(std::round(best_centroid.y))); - tracking_flag_ = true; - tracking_count_++; - lost_count_ = 0; - } else { - tracking_flag_ = false; - tracking_count_++; - } - - last_process_time_ms_ = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time).count(); - } - - cv::Point2i get_current_position() const { return current_position_; } - - bool get_tracking_status() const { return tracking_flag_; } - - std::string get_debug_info() const { - std::string info = "Tracker: "; - info += tracking_flag_ ? "Tracking" : "Lost"; - info += ", Pos=(" + std::to_string(current_position_.x) + - "," + std::to_string(current_position_.y) + ")"; - info += ", Tracked=" + std::to_string(tracking_count_); - info += ", Lost=" + std::to_string(lost_count_); - info += ", ValidContours=" + std::to_string(valid_contours_); - if (last_process_time_ms_ > 0) { - info += ", Time=" + std::to_string(last_process_time_ms_) + "ms"; - } - return info; - } - -private: - bool tracking_flag_; - cv::Point2i current_position_; - bool is_initialized_; - double min_contour_area_; - double max_distance_threshold_; - - int tracking_count_ = 0; - int lost_count_ = 0; - int valid_contours_ = 0; - int64_t last_process_time_ms_ = 0; - std::chrono::steady_clock::time_point last_track_time_; -}; -} // namespace rmcs_dart_guidance \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp deleted file mode 100644 index 8314a19c..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/test/debug_image_display.cpp +++ /dev/null @@ -1,321 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class DebugDisplayComponent - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DebugDisplayComponent() - : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { - - // Store parameters in member variables for later use - raw_image_topic_ = get_parameter("raw_image_topic").as_string(); - processed_image_topic_ = get_parameter("processed_image_topic").as_string(); - target_topic_ = get_parameter("target_topic").as_string(); - - display_raw_ = get_parameter("display_raw").as_bool(); - display_processed_ = get_parameter("display_processed").as_bool(); - max_fps_ = static_cast(get_parameter("max_fps").as_int()); - window_scale_ = get_parameter("window_scale").as_double(); - save_on_error_ = get_parameter("save_on_error").as_bool(); - save_directory_ = get_parameter("save_directory").as_string(); - - // Check if display is available - const char* display_env = std::getenv("DISPLAY"); - if (!display_env || std::string(display_env).empty()) { - RCLCPP_WARN(logger_, "DISPLAY environment variable not set. Display windows disabled."); - display_available_ = false; - } else { - display_available_ = true; - } - - RCLCPP_INFO(logger_, "DebugDisplayComponent constructed"); - } - - ~DebugDisplayComponent() { - stop_display(); - } - - void update() override { - // Initialize subscriptions and display on first update - if (!initialized_) { - initialize(); - initialized_ = true; - } - } - -private: - void initialize() { - RCLCPP_INFO(logger_, "Initializing DebugDisplayComponent"); - - // Create subscriptions - if (display_raw_) { - raw_image_sub_ = this->create_subscription( - raw_image_topic_, 10, - [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - raw_image_callback(msg); - }); - RCLCPP_INFO(logger_, "Subscribed to raw image topic: %s", raw_image_topic_.c_str()); - } - - if (display_processed_) { - processed_image_sub_ = this->create_subscription( - processed_image_topic_, 10, - [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - processed_image_callback(msg); - }); - RCLCPP_INFO(logger_, "Subscribed to processed image topic: %s", processed_image_topic_.c_str()); - - target_sub_ = this->create_subscription( - target_topic_, 10, - [this](const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { - target_callback(msg); - }); - RCLCPP_INFO(logger_, "Subscribed to target topic: %s", target_topic_.c_str()); - } - - // Start display thread if available - if (display_available_) { - display_running_ = true; - display_thread_ = std::thread(&DebugDisplayComponent::display_loop, this); - RCLCPP_INFO(logger_, "Display thread started"); - } - } - - void stop_display() { - display_running_ = false; - if (display_thread_.joinable()) { - display_thread_.join(); - RCLCPP_INFO(logger_, "Display thread stopped"); - } - - // Clean up OpenCV windows - if (display_raw_) { - cv::destroyWindow("Raw Image"); - } - if (display_processed_) { - cv::destroyWindow("Processed Image"); - } - } - - void raw_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) { - std::lock_guard lock(raw_mutex_); - try { - raw_image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; - raw_image_timestamp_ = std::chrono::steady_clock::now(); - } catch (const cv_bridge::Exception& e) { - RCLCPP_ERROR(logger_, "cv_bridge exception for raw image: %s", e.what()); - } - } - - void processed_image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - std::lock_guard lock(processed_mutex_); - try { - processed_image_ = cv_bridge::toCvCopy(msg, "mono8")->image; - processed_image_timestamp_ = std::chrono::steady_clock::now(); - } catch (const cv_bridge::Exception& e) { - RCLCPP_ERROR(logger_, "cv_bridge exception for processed image: %s", e.what()); - } - } - - void target_callback(const geometry_msgs::msg::PointStamped::ConstSharedPtr& msg) { - std::lock_guard lock(target_mutex_); - target_position_.x = static_cast(msg->point.x); - target_position_.y = static_cast(msg->point.y); - target_tracking_ = (msg->point.z > 0.5); - last_target_time_ = std::chrono::steady_clock::now(); - } - - void display_loop() { - // Create display windows - if (display_raw_) { - cv::namedWindow("Raw Image", cv::WINDOW_NORMAL); - cv::resizeWindow("Raw Image", - static_cast(640 * window_scale_), - static_cast(480 * window_scale_)); - } - - if (display_processed_) { - cv::namedWindow("Processed Image", cv::WINDOW_NORMAL); - cv::resizeWindow("Processed Image", - static_cast(640 * window_scale_), - static_cast(480 * window_scale_)); - } - - const int frame_delay_ms = 1000 / std::max(1, max_fps_); - int frame_counter = 0; - auto last_fps_time = std::chrono::steady_clock::now(); - - RCLCPP_INFO(logger_, "Display loop started with delay %d ms", frame_delay_ms); - - while (display_running_ && rclcpp::ok()) { - try { - // Display raw image - if (display_raw_) { - cv::Mat display_raw; - { - std::lock_guard lock(raw_mutex_); - if (!raw_image_.empty()) { - display_raw = raw_image_.clone(); - - // Check for stale data - auto now = std::chrono::steady_clock::now(); - auto elapsed = std::chrono::duration_cast( - now - raw_image_timestamp_).count(); - - if (elapsed > 500) { - cv::putText(display_raw, "STALE", cv::Point(10, 30), - cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); - } - } - } - - if (!display_raw.empty()) { - cv::imshow("Raw Image", display_raw); - } - } - - // Display processed image - if (display_processed_) { - cv::Mat display_processed; - { - std::lock_guard lock(processed_mutex_); - if (!processed_image_.empty()) { - display_processed = processed_image_.clone(); - - // Convert to color for visualization - if (display_processed.channels() == 1) { - cv::cvtColor(display_processed, display_processed, cv::COLOR_GRAY2BGR); - } - - // Add target information - { - std::lock_guard lock(target_mutex_); - if (target_tracking_) { - cv::circle(display_processed, target_position_, 20, - cv::Scalar(0, 255, 255), 2); - cv::putText(display_processed, "TRACKING", cv::Point(10, 30), - cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2); - } - } - - // Check for stale data - auto now = std::chrono::steady_clock::now(); - auto elapsed = std::chrono::duration_cast( - now - processed_image_timestamp_).count(); - - if (elapsed > 500) { - cv::putText(display_processed, "STALE", cv::Point(500, 30), - cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2); - } - } - } - - if (!display_processed.empty()) { - cv::imshow("Processed Image", display_processed); - } - } - - // Handle keyboard input - int key = cv::waitKey(frame_delay_ms) & 0xFF; - if (key == 27) { // ESC key - display_running_ = false; - RCLCPP_INFO(logger_, "ESC pressed, stopping display"); - } else if (key == 's' || key == 'S') { - save_current_frame(); - } - - frame_counter++; - - // Log FPS every 60 frames - if (frame_counter % 60 == 0) { - auto now = std::chrono::steady_clock::now(); - auto elapsed = std::chrono::duration_cast( - now - last_fps_time).count(); - double fps = 60000.0 / static_cast(std::max(1L, elapsed)); - RCLCPP_DEBUG(logger_, "Display FPS: %.1f", fps); - last_fps_time = now; - } - - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Display error: %s", e.what()); - } - } - - RCLCPP_INFO(logger_, "Display loop ended"); - } - - void save_current_frame() { - std::lock_guard lock(processed_mutex_); - if (!processed_image_.empty()) { - auto now = std::chrono::system_clock::now(); - auto time_t = std::chrono::system_clock::to_time_t(now); - char time_str[100]; - std::strftime(time_str, sizeof(time_str), "%Y%m%d_%H%M%S", std::localtime(&time_t)); - - std::string filename = save_directory_ + "/frame_" + time_str + ".jpg"; - cv::imwrite(filename, processed_image_); - RCLCPP_INFO(logger_, "Saved frame: %s", filename.c_str()); - } - } - -private: - rclcpp::Logger logger_; - - // Topic names - std::string raw_image_topic_; - std::string processed_image_topic_; - std::string target_topic_; - - // ROS2 subscribers - rclcpp::Subscription::SharedPtr raw_image_sub_; - rclcpp::Subscription::SharedPtr processed_image_sub_; - rclcpp::Subscription::SharedPtr target_sub_; - - // Image data - cv::Mat raw_image_; - cv::Mat processed_image_; - std::chrono::steady_clock::time_point raw_image_timestamp_; - std::chrono::steady_clock::time_point processed_image_timestamp_; - - // Target data - cv::Point target_position_; - bool target_tracking_ = false; - std::chrono::steady_clock::time_point last_target_time_; - - // Mutexes for thread safety - std::mutex raw_mutex_; - std::mutex processed_mutex_; - std::mutex target_mutex_; - - // Display thread - std::thread display_thread_; - std::atomic display_running_{true}; - std::atomic display_available_{true}; - std::atomic initialized_{false}; - - // Configuration parameters - bool display_raw_ = false; - bool display_processed_ = true; - int max_fps_ = 30; - double window_scale_ = 1.0; - bool save_on_error_ = false; - std::string save_directory_ = "./debug_saved"; -}; - -} // namespace rmcs_dart_guidance - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::DebugDisplayComponent, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp deleted file mode 100644 index 0a901b40..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/test/get_camera_parameter.cpp +++ /dev/null @@ -1,498 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class CameraCalibrationComponent - : public rmcs_executor::Component - , public rclcpp::Node { -public: - CameraCalibrationComponent() - : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { - - image_topic_ = get_parameter("image_topic").as_string(); - board_width_ = static_cast(get_parameter("board_width").as_int()); - board_height_ = static_cast(get_parameter("board_height").as_int()); - square_size_ = get_parameter("square_size").as_double(); - save_path_ = get_parameter("save_path").as_string(); - enabled_ = get_parameter("enabled").as_bool(); - - RCLCPP_INFO(logger_, "CameraCalibrationComponent: enabled=%d, save_path='%s'", enabled_.load(), save_path_.c_str()); - - if (!enabled_) { - RCLCPP_INFO(logger_, "Camera calibration disabled by config."); - return; - } - - if (board_width_ < 2 || board_height_ < 2) { - RCLCPP_ERROR(logger_, "Invalid board size: (%d, %d)", board_width_, board_height_); - enabled_ = false; - return; - } - - std::string final_path = save_path_; - try { - std::filesystem::create_directories(final_path); - RCLCPP_INFO(logger_, "Save directory created/verified: %s", final_path.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Failed to create save directory %s: %s", final_path.c_str(), e.what()); - enabled_ = false; - return; - } - - std::string succ_dir = final_path + "/success"; - try { - std::filesystem::create_directories(succ_dir); - RCLCPP_INFO(logger_, "Created success subdirectory"); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Failed to create success subdirectory: %s", e.what()); - enabled_ = false; - return; - } - - for (int i = 0; i < board_height_; ++i) { - for (int j = 0; j < board_width_; ++j) { - object_points_.emplace_back(j * square_size_, i * square_size_, 0.0f); - } - } - - pattern_size_ = cv::Size(board_width_, board_height_); - - RCLCPP_INFO(logger_, "CameraCalibrationComponent ready with board=%dx%d, square=%.3fm, pattern size=%dx%d", - board_width_, board_height_, square_size_, pattern_size_.width, pattern_size_.height); - RCLCPP_INFO(logger_, "Absolute save path: %s", std::filesystem::absolute(final_path).c_str()); - } - - ~CameraCalibrationComponent() { - stop_calibration(); - } - - void update() override { - if (!enabled_ || initialized_) return; - - image_sub_ = this->create_subscription( - image_topic_, 10, - [this](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - image_callback(msg); - }); - - RCLCPP_INFO(logger_, "Subscribed to image topic: %s", image_topic_.c_str()); - - calibration_running_ = true; - calibration_thread_ = std::thread(&CameraCalibrationComponent::calibration_loop, this); - initialized_ = true; - - RCLCPP_INFO(logger_, "Calibration thread started (automatic mode)"); - } - -private: - void stop_calibration() { - calibration_running_ = false; - if (calibration_thread_.joinable()) { - calibration_thread_.join(); - } - } - - void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr& msg) { - std::lock_guard lock(img_mutex_); - try { - current_image_ = cv_bridge::toCvCopy(msg, "bgr8")->image; - img_timestamp_ = std::chrono::steady_clock::now(); - RCLCPP_INFO(logger_, "Image received: %dx%d", current_image_.cols, current_image_.rows); - } catch (const cv_bridge::Exception& e) { - RCLCPP_ERROR(logger_, "cv_bridge exception: %s", e.what()); - } - } - - void calibration_loop() { - const int target_fps = 30; - const std::chrono::milliseconds frame_delay(1000 / target_fps); - RCLCPP_INFO(logger_, "Calibration loop started."); - - cv::Mat last_gray; - int frame_counter = 0; - int empty_counter = 0; - - while (calibration_running_ && rclcpp::ok()) { - cv::Mat frame; - { - std::lock_guard lock(img_mutex_); - if (!current_image_.empty()) { - frame = current_image_.clone(); - } - } - - if (!frame.empty()) { - frame_counter++; - cv::Mat gray; - cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); - - cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); - cv::Mat enhanced; - clahe->apply(gray, enhanced); - - std::vector corners; - bool found = false; - - found = cv::findChessboardCorners(enhanced, pattern_size_, corners, - cv::CALIB_CB_ADAPTIVE_THRESH + - cv::CALIB_CB_NORMALIZE_IMAGE + - cv::CALIB_CB_FAST_CHECK + - cv::CALIB_CB_EXHAUSTIVE); - - if (!found) { - found = cv::findChessboardCorners(gray, pattern_size_, corners, - cv::CALIB_CB_ADAPTIVE_THRESH + - cv::CALIB_CB_NORMALIZE_IMAGE + - cv::CALIB_CB_FAST_CHECK); - } - - if (found) { - cv::cornerSubPix(gray, corners, cv::Size(11,11), cv::Size(-1,-1), - cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.001)); - - bool is_new = true; - if (!last_gray.empty()) { - cv::Mat diff; - cv::absdiff(gray, last_gray, diff); - double mean_diff = cv::mean(diff)[0]; - if (mean_diff < 5.0) { - is_new = false; - } - } - - if (is_new) { - last_gray = gray.clone(); - { - std::lock_guard lock(capture_mutex_); - captured_image_points_.push_back(corners); - captured_object_points_.push_back(object_points_); - RCLCPP_INFO(logger_, "Captured frame #%zu", captured_image_points_.size()); - } - - cv::Mat display = frame.clone(); - cv::drawChessboardCorners(display, pattern_size_, corners, true); - std::string filename = save_path_ + "/success/frame_" + std::to_string(frame_counter) + ".jpg"; - if (cv::imwrite(filename, display)) { - RCLCPP_INFO(logger_, "Saved success image: %s", filename.c_str()); - } else { - RCLCPP_ERROR(logger_, "Failed to save success image: %s", filename.c_str()); - } - } - } - } else { - empty_counter++; - if (empty_counter % 30 == 0) { - RCLCPP_WARN(logger_, "No image received for %d frames", empty_counter); - } - } - - bool need_calibration = false; - - { - std::lock_guard lock(capture_mutex_); - if (static_cast(captured_image_points_.size()) >= min_capture_frames_) { - RCLCPP_INFO(logger_, "Collected %zu frames, stopping image reception and starting calibration...", - captured_image_points_.size()); - - image_sub_.reset(); - RCLCPP_INFO(logger_, "Image subscription cancelled."); - need_calibration = true; - } - } - - if (need_calibration) { - run_calibration(); - calibration_running_ = false; - break; - } - - std::this_thread::sleep_for(frame_delay); - } - RCLCPP_INFO(logger_, "Calibration loop ended."); - } - - void run_calibration() { - std::lock_guard lock(capture_mutex_); - - RCLCPP_INFO(logger_, "run_calibration entered with %zu frames", captured_image_points_.size()); - - if (captured_image_points_.size() < 5) { - RCLCPP_WARN(logger_, "Not enough frames captured (%zu). Need at least 5.", captured_image_points_.size()); - return; - } - - RCLCPP_INFO(logger_, "Running calibration with %zu frames...", captured_image_points_.size()); - - cv::Mat camera_matrix = cv::Mat::eye(3, 3, CV_64F); - cv::Mat dist_coeffs; - std::vector rvecs, tvecs; - double rms = -1.0; - - cv::Size image_size; - { - std::lock_guard lock(img_mutex_); - if (!current_image_.empty()) { - image_size = current_image_.size(); - RCLCPP_INFO(logger_, "Image size for calibration: %dx%d", image_size.width, image_size.height); - } else { - RCLCPP_ERROR(logger_, "No image available to determine size."); - return; - } - } - - RCLCPP_INFO(logger_, "Calling cv::calibrateCamera..."); - try { - rms = cv::calibrateCamera( - captured_object_points_, - captured_image_points_, - image_size, - camera_matrix, - dist_coeffs, - rvecs, - tvecs - ); - RCLCPP_INFO(logger_, "calibrateCamera completed with RMS = %.4f", rms); - } catch (const cv::Exception& e) { - RCLCPP_ERROR(logger_, "Calibration failed with OpenCV exception: %s", e.what()); - return; - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Calibration failed with standard exception: %s", e.what()); - return; - } - - if (rms < 0 || rms > 10.0) { - RCLCPP_WARN(logger_, "Calibration RMS (%.4f) seems too high, results may be unreliable.", rms); - } - double fx = camera_matrix.at(0,0); - double fy = camera_matrix.at(1,1); - if (fx <= 0 || fy <= 0) { - RCLCPP_ERROR(logger_, "Invalid focal length (fx=%.4f, fy=%.4f). Calibration failed.", fx, fy); - return; - } - - RCLCPP_INFO(logger_, "Calibration RMS: %.4f pixels", rms); - RCLCPP_INFO(logger_, "Camera matrix:\n%s", camera_matrixToString(camera_matrix).c_str()); - RCLCPP_INFO(logger_, "Distortion coefficients: %s", dist_coeffsToString(dist_coeffs).c_str()); - - saveCalibrationResults(camera_matrix, dist_coeffs, image_size, rms); - } - - static std::string camera_matrixToString(const cv::Mat& K) { - std::ostringstream oss; - oss << std::fixed << std::setprecision(4); - oss << "[ " << K.at(0,0) << ", " << K.at(0,1) << ", " << K.at(0,2) << ";\n"; - oss << " " << K.at(1,0) << ", " << K.at(1,1) << ", " << K.at(1,2) << ";\n"; - oss << " " << K.at(2,0) << ", " << K.at(2,1) << ", " << K.at(2,2) << " ]"; - return oss.str(); - } - - static std::string dist_coeffsToString(const cv::Mat& D) { - std::ostringstream oss; - oss << std::fixed << std::setprecision(6); - oss << "[ "; - for (int i = 0; i < static_cast(D.total()); ++i) { - if (i > 0) oss << ", "; - oss << D.at(i); - } - oss << " ]"; - return oss.str(); - } - - void saveCalibrationResults(const cv::Mat& camera_matrix, const cv::Mat& dist_coeffs, - const cv::Size& image_size, double rms) { - RCLCPP_INFO(logger_, "saveCalibrationResults started"); - - try { - std::filesystem::create_directories(save_path_); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Cannot create directory %s: %s", save_path_.c_str(), e.what()); - return; - } - - auto now = std::chrono::system_clock::now(); - auto time_t = std::chrono::system_clock::to_time_t(now); - std::tm tm_buf; - localtime_r(&time_t, &tm_buf); - - // 1. Save as YAML - { - std::ostringstream oss; - oss << save_path_ << "/calibration_" - << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".yaml"; - std::string filename = oss.str(); - std::string abs_path = std::filesystem::absolute(filename).string(); - RCLCPP_INFO(logger_, "Saving YAML to: %s", abs_path.c_str()); - - cv::FileStorage fs(filename, cv::FileStorage::WRITE); - if (!fs.isOpened()) { - RCLCPP_ERROR(logger_, "Failed to open YAML file for writing: %s", filename.c_str()); - } else { - fs << "camera_matrix" << camera_matrix; - fs << "distortion_coefficients" << dist_coeffs; - fs << "image_width" << image_size.width; - fs << "image_height" << image_size.height; - fs << "rms" << rms; - fs << "board_width" << board_width_; - fs << "board_height" << board_height_; - fs << "square_size" << square_size_; - fs.release(); - if (std::filesystem::exists(filename)) { - RCLCPP_INFO(logger_, "YAML saved successfully"); - } else { - RCLCPP_ERROR(logger_, "YAML file not found after save"); - } - } - } - - // 2. Save as C++ header file - { - std::ostringstream oss; - oss << save_path_ << "/calibration_" - << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".hpp"; - std::string filename = oss.str(); - std::ofstream ofs(filename); - if (ofs) { - ofs << "#pragma once\n\n"; - ofs << "namespace calibration_result {\n\n"; - ofs << "constexpr double camera_fx = " << camera_matrix.at(0,0) << ";\n"; - ofs << "constexpr double camera_fy = " << camera_matrix.at(1,1) << ";\n"; - ofs << "constexpr double camera_cx = " << camera_matrix.at(0,2) << ";\n"; - ofs << "constexpr double camera_cy = " << camera_matrix.at(1,2) << ";\n\n"; - ofs << "constexpr double distortion_k1 = " << dist_coeffs.at(0) << ";\n"; - ofs << "constexpr double distortion_k2 = " << dist_coeffs.at(1) << ";\n"; - ofs << "constexpr double distortion_p1 = " << dist_coeffs.at(2) << ";\n"; - ofs << "constexpr double distortion_p2 = " << dist_coeffs.at(3) << ";\n"; - if (dist_coeffs.total() > 4) - ofs << "constexpr double distortion_k3 = " << dist_coeffs.at(4) << ";\n"; - ofs << "\n} // namespace calibration_result\n"; - ofs.close(); - RCLCPP_INFO(logger_, "C++ header saved: %s", filename.c_str()); - } else { - RCLCPP_ERROR(logger_, "Failed to save C++ header: %s", filename.c_str()); - } - } - - // 3. Save as binary (OpenCV bin) - { - std::ostringstream oss; - oss << save_path_ << "/calibration_" - << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".bin"; - std::string filename = oss.str(); - cv::FileStorage fs(filename, cv::FileStorage::WRITE + cv::FileStorage::FORMAT_XML); - if (!fs.isOpened()) { - RCLCPP_ERROR(logger_, "Failed to open binary file for writing: %s", filename.c_str()); - } else { - fs << "camera_matrix" << camera_matrix; - fs << "distortion_coefficients" << dist_coeffs; - fs << "image_width" << image_size.width; - fs << "image_height" << image_size.height; - fs << "rms" << rms; - fs << "board_width" << board_width_; - fs << "board_height" << board_height_; - fs << "square_size" << square_size_; - fs.release(); - RCLCPP_INFO(logger_, "Binary file saved: %s", filename.c_str()); - } - } - - // 4. Save as Markdown - { - std::ostringstream oss; - oss << save_path_ << "/calibration_" - << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") << ".md"; - std::string filename = oss.str(); - std::ofstream ofs(filename); - if (ofs) { - ofs << "# Camera Calibration Result\n\n"; - ofs << "Date: " << std::put_time(&tm_buf, "%Y-%m-%d %H:%M:%S") << "\n\n"; - ofs << "## Board Parameters\n"; - ofs << "- Board size: " << board_width_ << " x " << board_height_ << " inner corners\n"; - ofs << "- Square size: " << square_size_ << " m\n\n"; - ofs << "## Camera Matrix (fx, fy, cx, cy)\n"; - ofs << "| fx | fy | cx | cy |\n"; - ofs << "|---|---|---|---|\n"; - ofs << "| " << camera_matrix.at(0,0) << " | " - << camera_matrix.at(1,1) << " | " - << camera_matrix.at(0,2) << " | " - << camera_matrix.at(1,2) << " |\n\n"; - ofs << "## Distortion Coefficients\n"; - ofs << "| k1 | k2 | p1 | p2 | k3 |\n"; - ofs << "|---|---|---|---|---|\n"; - ofs << "| " << dist_coeffs.at(0) << " | " - << dist_coeffs.at(1) << " | " - << dist_coeffs.at(2) << " | " - << dist_coeffs.at(3) << " | "; - if (dist_coeffs.total() > 4) - ofs << dist_coeffs.at(4); - else - ofs << "0.0"; - ofs << " |\n\n"; - ofs << "## Reprojection Error (RMS)\n"; - ofs << "- RMS = " << rms << " pixels\n"; - ofs.close(); - RCLCPP_INFO(logger_, "Markdown file saved: %s", filename.c_str()); - } else { - RCLCPP_ERROR(logger_, "Failed to save Markdown file: %s", filename.c_str()); - } - } - - RCLCPP_INFO(logger_, "All calibration result files saved to %s", save_path_.c_str()); - RCLCPP_INFO(logger_, "\nParameters for DartVisionCore:\n" - "camera_focal_length_x: %.4f\n" - "camera_focal_length_y: %.4f\n" - "camera_principal_point_x: %.4f\n" - "camera_principal_point_y: %.4f\n", - camera_matrix.at(0,0), - camera_matrix.at(1,1), - camera_matrix.at(0,2), - camera_matrix.at(1,2)); - } - -private: - rclcpp::Logger logger_; - - rclcpp::Subscription::SharedPtr image_sub_; - std::string image_topic_; - - int board_width_; - int board_height_; - double square_size_; - cv::Size pattern_size_; - std::vector object_points_; - std::vector> captured_object_points_; - std::vector> captured_image_points_; - const int min_capture_frames_ = 10; - - std::string save_path_; - - cv::Mat current_image_; - std::chrono::steady_clock::time_point img_timestamp_; - std::mutex img_mutex_; - std::mutex capture_mutex_; - - std::thread calibration_thread_; - std::atomic calibration_running_{false}; - std::atomic enabled_{false}; - std::atomic initialized_{false}; -}; - -} // namespace rmcs_dart_guidance - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::CameraCalibrationComponent, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp deleted file mode 100644 index 543699fb..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/test/image_publisher.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class ImagePublisher - : public rmcs_executor::Component - , public rclcpp::Node { -public: - ImagePublisher() - : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { - - register_input(get_parameter("Interface_name").as_string(), input_image_); - - image_publisher_ = - this->create_publisher(get_parameter("topic_name").as_string(), 1000); - - publish_freq_ = get_parameter("publish_freq").as_double(); - publish_freq_ = MAX(0, MIN(publish_freq_, 1000)); - image_type_ = get_parameter("image_type").as_string(); - timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(1000 / publish_freq_)), [this]() { - sensor_msgs::msg::Image publish_image_msg; - cv_bridge::CvImage cv_image(std_msgs::msg::Header(), image_type_, *input_image_); - publish_image_msg = *cv_image.toImageMsg(); - image_publisher_->publish(publish_image_msg); - }); - } - - void update() override {} - -private: - rclcpp::Logger logger_; - - double publish_freq_ = 60.0; // Hz - std::string image_type_; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr image_publisher_; - InputInterface input_image_; -}; -} // namespace rmcs_dart_guide - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::ImagePublisher, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp b/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp deleted file mode 100644 index 7e4cce15..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance/test/image_saving_test.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_dart_guidance { - -class TestImageSaver : public rmcs_executor::Component, public rclcpp::Node { -public: - TestImageSaver() - : Node(get_component_name(), rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , logger_(get_logger()) { - - // 读取测试参数 - std::string save_dir = "./test_images"; - if (has_parameter("save_directory")) { - save_dir = get_parameter("save_directory").as_string(); - } - - int test_count = 5; - if (has_parameter("test_count")) { - test_count = static_cast(get_parameter("test_count").as_int()); - } - - int interval_ms = 1000; - if (has_parameter("interval_ms")) { - interval_ms = static_cast(get_parameter("interval_ms").as_int()); - } - - RCLCPP_INFO(logger_, "Starting image saver test:"); - RCLCPP_INFO(logger_, " Directory: %s", save_dir.c_str()); - RCLCPP_INFO(logger_, " Count: %d", test_count); - RCLCPP_INFO(logger_, " Interval: %d ms", interval_ms); - - // 创建目录 - try { - std::filesystem::create_directories(save_dir); - RCLCPP_INFO(logger_, "Created directory: %s", save_dir.c_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Failed to create directory: %s", e.what()); - return; - } - - // 生成测试图像并保存 - for (int i = 0; i < test_count; ++i) { - test_save_image(save_dir, i); - std::this_thread::sleep_for(std::chrono::milliseconds(interval_ms)); - } - - RCLCPP_INFO(logger_, "Test completed. Check directory: %s", save_dir.c_str()); - } - - void update() override { - // 测试完成后停止更新 - } - -private: - void test_save_image(const std::string& directory, int index) { - // 创建测试图像 - cv::Mat test_image(480, 640, CV_8UC3, cv::Scalar(0, 0, 0)); - - // 添加一些内容 - cv::rectangle(test_image, cv::Point(100, 100), cv::Point(540, 380), cv::Scalar(0, 255, 0), 2); - cv::circle(test_image, cv::Point(320, 240), 50, cv::Scalar(255, 0, 0), -1); - cv::putText(test_image, "Test " + std::to_string(index), cv::Point(200, 400), - cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(255, 255, 255), 2); - - // 生成文件名 - auto now = std::chrono::system_clock::now(); - auto time_t = std::chrono::system_clock::to_time_t(now); - std::tm tm_buf; - localtime_r(&time_t, &tm_buf); - - std::ostringstream oss; - oss << directory << "/test_" - << std::put_time(&tm_buf, "%Y%m%d_%H%M%S") - << "_" << std::setw(3) << std::setfill('0') << index - << ".png"; - - std::string filename = oss.str(); - - // 保存图像 - try { - bool success = cv::imwrite(filename, test_image); - if (success) { - RCLCPP_INFO(logger_, "Saved test image: %s", filename.c_str()); - - // 验证文件存在 - if (std::filesystem::exists(filename)) { - auto file_size = std::filesystem::file_size(filename); - RCLCPP_INFO(logger_, " File size: %ld bytes", file_size); - } else { - RCLCPP_ERROR(logger_, " File not found after saving!"); - } - } else { - RCLCPP_ERROR(logger_, "Failed to save image: %s", filename.c_str()); - } - } catch (const cv::Exception& e) { - RCLCPP_ERROR(logger_, "OpenCV exception: %s", e.what()); - } catch (const std::exception& e) { - RCLCPP_ERROR(logger_, "Exception: %s", e.what()); - } - } - - rclcpp::Logger logger_; -}; - -} // namespace rmcs_dart_guidance - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_dart_guidance::TestImageSaver, rmcs_executor::Component) \ No newline at end of file From 494dbf06191d9c2ead5d11d1256f28eb0a8b66cb Mon Sep 17 00:00:00 2001 From: hyperlee <13869662005@163.com> Date: Sun, 8 Mar 2026 20:00:16 +0800 Subject: [PATCH 31/45] add rmcs_dart_guidance as submodule --- .gitmodules | 3 +++ rmcs_ws/src/rmcs_dart_guidance | 1 + 2 files changed, 4 insertions(+) create mode 160000 rmcs_ws/src/rmcs_dart_guidance diff --git a/.gitmodules b/.gitmodules index d4d71be2..5ad90e84 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,3 +10,6 @@ [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git +[submodule "rmcs_ws/src/rmcs_dart_guidance"] + path = rmcs_ws/src/rmcs_dart_guidance + url = https://github.com/hyperle/rmcs_submodule_dart_guidance.git diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance new file mode 160000 index 00000000..e00dea1c --- /dev/null +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -0,0 +1 @@ +Subproject commit e00dea1c0ddc9ccadb239398f82d647afbd0d5be From c685b54579ec11f234f07c7720edc6d5fd0cb752 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 11 Mar 2026 02:01:44 +0800 Subject: [PATCH 32/45] Partial transplant librmcs v3.1.0 --- rmcs_ws/src/rmcs_core/CMakeLists.txt | 25 +- rmcs_ws/src/rmcs_core/librmcs | 2 +- .../rmcs_core/src/hardware/device/bmi088.hpp | 163 +++++- .../src/hardware/device/can_packet.hpp | 67 +++ .../src/hardware/device/dji_motor.hpp | 218 +++++++- .../src/hardware/device/lk_motor.hpp | 502 ++++++++++++++++-- .../include/rmcs_utility/as_byte_span.hpp | 7 + .../include/rmcs_utility/endian_promise.hpp | 194 +++++++ 8 files changed, 1088 insertions(+), 90 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpp create mode 100644 rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index 0a7372dd..e1f8918a 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -3,33 +3,38 @@ project(rmcs_core) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_STANDARD 23) +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) endif() -find_package (ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies () +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.1.0b0/librmcs-sdk-src-3.1.0-beta.0.zip + DOWNLOAD_EXTRACT_TIMESTAMP TRUE +) +FetchContent_MakeAvailable(librmcs) -file (GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS +file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp ${PROJECT_SOURCE_DIR}/src/*.c ) -ament_auto_add_library ( +ament_auto_add_library( ${PROJECT_NAME} SHARED ${PROJECT_SOURCE} ) include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) -include_directories(${PROJECT_SOURCE_DIR}/librmcs) - -include_directories(SYSTEM "/usr/include/libusb-1.0") - -target_link_libraries(${PROJECT_NAME} -lusb-1.0) +target_link_libraries(${PROJECT_NAME} librmcs-sdk) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index b82f2eaf..eff14b71 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 +Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp index 91351a18..87a0b9a2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp @@ -1,9 +1,168 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include namespace rmcs_core::hardware::device { -using Bmi088 = librmcs::device::Bmi088; +class Bmi088 { +public: + explicit Bmi088( + double sample_freq, double kp, double ki, double q0 = 1, double q1 = 0, double q2 = 0, + double q3 = 0) + : inv_sample_freq_(1.0 / sample_freq) + , double_kp_(2.0 * kp) + , double_ki_(2.0 * ki) + , q0_(q0) + , q1_(q1) + , q2_(q2) + , q3_(q3) {} + + void set_coordinate_mapping( + std::function(double, double, double)> + mapping_function) { + coordinate_mapping_function_ = std::move(mapping_function); + } + + void store_accelerometer_status(int16_t x, int16_t y, int16_t z) { + accelerometer_data_.store({x, y, z}, std::memory_order::relaxed); + } + + void store_gyroscope_status(int16_t x, int16_t y, int16_t z) { + gyroscope_data_.store({x, y, z}, std::memory_order::relaxed); + } + + void update_status() { + auto acc = accelerometer_data_.load(std::memory_order::relaxed); + auto gyro = gyroscope_data_.load(std::memory_order::relaxed); + + auto solve_acc = [](int16_t value) { return value / 32767.0 * 6.0; }; + auto solve_gyro = [](int16_t value) { + return value / 32767.0 * 2000.0 / 180.0 * std::numbers::pi; + }; + + gx_ = solve_gyro(gyro.x), gy_ = solve_gyro(gyro.y), gz_ = solve_gyro(gyro.z); + ax_ = solve_acc(acc.x), ay_ = solve_acc(acc.y), az_ = solve_acc(acc.z); + + if (coordinate_mapping_function_) { + std::tie(gx_, gy_, gz_) = coordinate_mapping_function_(gx_, gy_, gz_); + std::tie(ax_, ay_, az_) = coordinate_mapping_function_(ax_, ay_, az_); + } + + mahony_ahrs_update_imu(ax_, ay_, az_, gx_, gy_, gz_); + } + + double ax() const { return ax_; } + double ay() const { return ay_; } + double az() const { return az_; } + + double gx() const { return gx_; } + double gy() const { return gy_; } + double gz() const { return gz_; } + + double& q0() { return q0_; } + double& q1() { return q1_; } + double& q2() { return q2_; } + double& q3() { return q3_; } + +private: + void mahony_ahrs_update_imu(double ax, double ay, double az, double gx, double gy, double gz) { + // Madgwick's implementation of Mayhony's AHRS algorithm. + // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + + double recip_norm; + double halfvx, halfvy, halfvz; + double halfex, halfey, halfez; + double qa, qb, qc; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer + // normalization) + if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) { + + // Normalize accelerometer measurement + recip_norm = 1 / std::sqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = q1_ * q3_ - q0_ * q2_; + halfvy = q0_ * q1_ + q2_ * q3_; + halfvz = q0_ * q0_ - 0.5 + q3_ * q3_; + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = ay * halfvz - az * halfvy; + halfey = az * halfvx - ax * halfvz; + halfez = ax * halfvy - ay * halfvx; + + // Compute and apply integral feedback if enabled + if (double_ki_ > 0.0) { + // integral error scaled by Ki + integral_fbx_ += double_ki_ * halfex * (inv_sample_freq_); + integral_fby_ += double_ki_ * halfey * (inv_sample_freq_); + integral_fbz_ += double_ki_ * halfez * (inv_sample_freq_); + // apply integral feedback + gx += integral_fbx_; + gy += integral_fby_; + gz += integral_fbz_; + } else { + // prevent integral windup + integral_fbx_ = 0.0; + integral_fby_ = 0.0; + integral_fbz_ = 0.0; + } + + // Apply proportional feedback + gx += double_kp_ * halfex; + gy += double_kp_ * halfey; + gz += double_kp_ * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5 * (inv_sample_freq_)); // pre-multiply common factors + gy *= (0.5 * (inv_sample_freq_)); + gz *= (0.5 * (inv_sample_freq_)); + qa = q0_; + qb = q1_; + qc = q2_; + q0_ += (-qb * gx - qc * gy - q3_ * gz); + q1_ += (qa * gx + qc * gz - q3_ * gy); + q2_ += (qa * gy - qb * gz + q3_ * gx); + q3_ += (qa * gz + qb * gy - qc * gx); + + // Normalize quaternion + recip_norm = 1 / std::sqrt(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_); + q0_ *= recip_norm; + q1_ *= recip_norm; + q2_ *= recip_norm; + q3_ *= recip_norm; + } + + double inv_sample_freq_; // The reciprocal of sampling frequency + double double_kp_; // 2 * proportional gain (Kp) + double double_ki_; // 2 * integral gain (Ki) + + struct alignas(8) ImuData { + int16_t x, y, z; + }; + std::atomic accelerometer_data_, gyroscope_data_; + static_assert(std::atomic::is_always_lock_free); + + double ax_, ay_, az_, gx_, gy_, gz_; + + std::function(double, double, double)> + coordinate_mapping_function_; + + // Quaternion of sensor frame relative to auxiliary frame + double q0_, q1_, q2_, q3_; + + // Integral error terms scaled by Ki + double integral_fbx_ = 0.0, integral_fby_ = 0.0, integral_fbz_ = 0.0; +}; } // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp new file mode 100644 index 00000000..ffaad288 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/can_packet.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +template +requires(std::is_trivial_v) struct ByteConvertible { + alignas(align) T data; + + ByteConvertible() = default; + + constexpr explicit ByteConvertible(const T& data) + : data(data) {} + + constexpr explicit ByteConvertible(std::span bytes) noexcept { + std::memcpy(&data, bytes.data(), sizeof(data)); + } + + constexpr explicit ByteConvertible(std::span bytes) { + if (bytes.size() != sizeof(data)) [[unlikely]] + throw std::invalid_argument("Illegal span size"); + + std::memcpy(&data, bytes.data(), sizeof(data)); + } + + constexpr std::span as_bytes() noexcept { + return std::span{ + reinterpret_cast(&data), sizeof(data)}; + } + + constexpr std::span as_writable_bytes() noexcept { + return std::span{ + reinterpret_cast(&data), sizeof(data)}; + } +}; + +struct CanPacket8 : ByteConvertible, alignof(uint64_t)> { + struct Quarter : ByteConvertible { + using ByteConvertible::ByteConvertible; + }; + + struct PaddingQuarter : Quarter { + PaddingQuarter() + : Quarter(0) {} + }; + + using ByteConvertible::ByteConvertible; + + explicit CanPacket8(uint64_t data) + : ByteConvertible(std::bit_cast(data)) {} + + CanPacket8(Quarter q0, Quarter q1, Quarter q2, Quarter q3) + : ByteConvertible({q0.data, q1.data, q2.data, q3.data}) {} +}; +static_assert(std::atomic::is_always_lock_free); +static_assert(std::atomic::is_always_lock_free); + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index 57a9f766..b7f4009e 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -1,20 +1,63 @@ #pragma once -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include + +#include "hardware/device/can_packet.hpp" namespace rmcs_core::hardware::device { -class DjiMotor : public librmcs::device::DjiMotor { +class DjiMotor { public: + enum class Type : uint8_t { kGM6020, kGM6020Voltage, kM3508, kM2006 }; + + struct Config { + explicit Config(Type motor_type) + : motor_type(motor_type) { + switch (motor_type) { + case Type::kGM6020: + case Type::kGM6020Voltage: reduction_ratio = 1.0; break; + case Type::kM3508: reduction_ratio = 3591.0 / 187.0; break; + case Type::kM2006: reduction_ratio = 36.0; break; + } + this->reversed = false; + this->multi_turn_angle_enabled = false; + } + + Config& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + Config& set_reduction_ratio(double value) { return reduction_ratio = value, *this; } + Config& set_reversed() { return reversed = true, *this; } + Config& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } + + Type motor_type; + int encoder_zero_point = 0; + double reduction_ratio; + bool reversed; + bool multi_turn_angle_enabled; + }; + DjiMotor( rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, const std::string& name_prefix) - : librmcs::device::DjiMotor() { - status_component.register_output(name_prefix + "/angle", angle_, 0.0); - status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); - status_component.register_output(name_prefix + "/torque", torque_, 0.0); - status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + : angle_(0.0) + , velocity_(0.0) + , torque_(0.0) { + status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); command_component.register_input(name_prefix + "/control_torque", control_torque_, false); } @@ -26,17 +69,106 @@ class DjiMotor : public librmcs::device::DjiMotor { configure(config); } + DjiMotor(const DjiMotor&) = delete; + DjiMotor& operator=(const DjiMotor&) = delete; + DjiMotor(DjiMotor&&) = delete; + DjiMotor& operator=(DjiMotor&&) = delete; + + ~DjiMotor() = default; + void configure(const Config& config) { - librmcs::device::DjiMotor::configure(config); + encoder_zero_point_ = config.encoder_zero_point % kRawAngleMax; + if (encoder_zero_point_ < 0) + encoder_zero_point_ += kRawAngleMax; + + const double sign = config.reversed ? -1 : 1; + + raw_angle_to_angle_coefficient_ = + sign / config.reduction_ratio / kRawAngleMax * 2 * std::numbers::pi; + angle_to_raw_angle_coefficient_ = 1 / raw_angle_to_angle_coefficient_; + + raw_velocity_to_velocity_coefficient_ = + sign / config.reduction_ratio / 60 * 2 * std::numbers::pi; + velocity_to_raw_velocity_coefficient_ = 1 / raw_velocity_to_velocity_coefficient_; + + double torque_constant, raw_current_max, current_max; + switch (config.motor_type) { + case Type::kGM6020: + torque_constant = 0.741; + raw_current_max = 16384.0; + current_max = 3.0; + break; + case Type::kGM6020Voltage: + torque_constant = 0.741; + raw_current_max = 25000.0; + current_max = 3.0; + break; + case Type::kM3508: + torque_constant = 0.3 * 187.0 / 3591.0; + raw_current_max = 16384.0; + current_max = 20.0; + break; + case Type::kM2006: + torque_constant = 0.18 * 1.0 / 36.0; + raw_current_max = 16384.0; + current_max = 10.0; + break; + default: std::unreachable(); + } + + raw_current_to_torque_coefficient_ = + sign * config.reduction_ratio * torque_constant / raw_current_max * current_max; + torque_to_raw_current_coefficient_ = 1 / raw_current_to_torque_coefficient_; + + max_torque_ = 1 * config.reduction_ratio * torque_constant * current_max; + + last_raw_angle_ = 0; + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + angle_multi_turn_ = 0; + + *max_torque_output_ = max_torque(); + } - *max_torque_ = max_torque(); + void store_status(std::span can_data) { + can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); } void update_status() { - librmcs::device::DjiMotor::update_status(); - *angle_ = angle(); - *velocity_ = velocity(); - *torque_ = torque(); + const auto feedback = + std::bit_cast(can_data_.load(std::memory_order::relaxed)); + + // Temperature unit: celsius + temperature_ = static_cast(feedback.temperature); + + // Angle unit: rad + const int raw_angle = feedback.angle; + int calibrated_raw_angle = raw_angle - encoder_zero_point_; + if (calibrated_raw_angle < 0) + calibrated_raw_angle += kRawAngleMax; + if (!multi_turn_angle_enabled_) { + angle_ = raw_angle_to_angle_coefficient_ * static_cast(calibrated_raw_angle); + if (angle_ < 0) + angle_ += 2 * std::numbers::pi; + } else { + auto diff = (calibrated_raw_angle - angle_multi_turn_) % kRawAngleMax; + if (diff <= -kRawAngleMax / 2) + diff += kRawAngleMax; + else if (diff > kRawAngleMax / 2) + diff -= kRawAngleMax; + angle_multi_turn_ += diff; + angle_ = raw_angle_to_angle_coefficient_ * static_cast(angle_multi_turn_); + } + last_raw_angle_ = raw_angle; + + // Velocity unit: rad/s + velocity_ = raw_velocity_to_velocity_coefficient_ * static_cast(feedback.velocity); + + // Torque unit: N*m + torque_ = raw_current_to_torque_coefficient_ * static_cast(feedback.current); + + *angle_output_ = angle(); + *velocity_output_ = velocity(); + *torque_output_ = torque(); } double control_torque() const { @@ -46,15 +178,63 @@ class DjiMotor : public librmcs::device::DjiMotor { return 0.0; } - uint16_t generate_command() { - return librmcs::device::DjiMotor::generate_command(control_torque()); + CanPacket8::Quarter generate_command() const { return generate_command(control_torque()); } + + CanPacket8::Quarter generate_command(double control_torque) const { + if (std::isnan(control_torque)) { + return CanPacket8::Quarter{0}; + } + + control_torque = std::clamp(control_torque, -max_torque_, max_torque_); + const double current = std::round(torque_to_raw_current_coefficient_ * control_torque); + const rmcs_utility::be_int16_t control_current = static_cast(current); + + return std::bit_cast(control_current); } + int calibrate_zero_point() { + angle_multi_turn_ = 0; + encoder_zero_point_ = last_raw_angle_; + return encoder_zero_point_; + } + + double angle() const { return angle_; } + double velocity() const { return velocity_; } + double torque() const { return torque_; } + double max_torque() const { return max_torque_; } + double temperature() const { return temperature_; } + private: - rmcs_executor::Component::OutputInterface angle_; - rmcs_executor::Component::OutputInterface velocity_; - rmcs_executor::Component::OutputInterface torque_; - rmcs_executor::Component::OutputInterface max_torque_; + struct alignas(uint64_t) DjiMotorFeedback { + rmcs_utility::be_int16_t angle; + rmcs_utility::be_int16_t velocity; + rmcs_utility::be_int16_t current; + uint8_t temperature; + uint8_t unused; + }; + + std::atomic can_data_; + + static constexpr int kRawAngleMax = 8192; + int encoder_zero_point_, last_raw_angle_; + + bool multi_turn_angle_enabled_; + int64_t angle_multi_turn_; + + double raw_angle_to_angle_coefficient_, angle_to_raw_angle_coefficient_; + double raw_velocity_to_velocity_coefficient_, velocity_to_raw_velocity_coefficient_; + double raw_current_to_torque_coefficient_, torque_to_raw_current_coefficient_; + + double angle_; + double velocity_; + double torque_; + double max_torque_; + double temperature_; + + rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface velocity_output_; + rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface max_torque_output_; rmcs_executor::Component::InputInterface control_torque_; }; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 19b6d9de..b921bc97 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 @@ -1,25 +1,56 @@ #pragma once +#include +#include +#include +#include +#include +#include +#include #include +#include +#include +#include +#include +#include -#include #include #include #include +#include "hardware/device/can_packet.hpp" + namespace rmcs_core::hardware::device { -class LkMotor : public librmcs::device::LkMotor { +class LkMotor { public: + enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10 }; + + struct Config { + explicit Config(Type type) + : motor_type(type) {} + + Config& set_encoder_zero_point(int value) { + RCLCPP_INFO(rclcpp::get_logger("awa"), "%d", value); + return encoder_zero_point = value, *this; + } + Config& set_reversed() { return reversed = true, *this; } + Config& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } + + Type motor_type; + int encoder_zero_point = 0; + bool reversed = false; + bool multi_turn_angle_enabled = false; + }; + LkMotor( rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, - const std::string& name_prefix) - : librmcs::device::LkMotor() { - status_component.register_output(name_prefix + "/angle", angle_, 0.0); - status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); - status_component.register_output(name_prefix + "/torque", torque_, 0.0); - status_component.register_output(name_prefix + "/temperature", temperature_, 0.0); - status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + const std::string& name_prefix) { + status_component.register_output(name_prefix + "/angle", angle_output_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_output_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_output_, 0.0); + status_component.register_output(name_prefix + "/temperature", temperature_output_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_output_, 0.0); command_component.register_input( // name_prefix + "/control_torque", control_torque_, false); @@ -39,17 +70,346 @@ class LkMotor : public librmcs::device::LkMotor { } void configure(const Config& config) { - librmcs::device::LkMotor::configure(config); + multi_turn_encoder_count_ = 0; + last_raw_angle_ = 0; + + double current_max; + double torque_constant; + double reduction_ratio; + + switch (config.motor_type) { + case Type::kMG5010Ei10: + raw_angle_max_ = 65535; + current_max = 33.0; + torque_constant = 0.90909; + reduction_ratio = 10.0; + + // Note: max_torque_ should represent the ACTUAL maximum torque of the motor. + // This value must be taken directly from the manufacturer's documentation. + // It is not used in calculations and serves as a reference only. + // Avoid calculating it by simply multiplying the maximum current by the torque + // constant, as this approach leads to inaccurate and unreliable results. + max_torque_ = 7.0; + break; + case Type::kMG4010Ei10: + raw_angle_max_ = 65535; + current_max = 33.0; + torque_constant = 0.07; + reduction_ratio = 10.0; + max_torque_ = 4.5; + break; + case Type::kMG6012Ei8: + raw_angle_max_ = 65535; + current_max = 33.0; + torque_constant = 1.09; + reduction_ratio = 8.0; + max_torque_ = 16.0; + break; + case Type::kMG4005Ei10: + raw_angle_max_ = 65535; + current_max = 33.0; + torque_constant = 0.06; + reduction_ratio = 10.0; + max_torque_ = 2.5; + break; + default: std::unreachable(); + } + + // Make sure raw_angle_max_ is a power of 2 + encoder_zero_point_ = config.encoder_zero_point & (raw_angle_max_ - 1); + + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + + const double sign = config.reversed ? -1.0 : 1.0; + + status_angle_to_angle_coefficient_ = sign / raw_angle_max_ * 2 * std::numbers::pi; + angle_to_command_angle_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0; - *max_torque_ = max_torque(); + status_velocity_to_velocity_coefficient_ = sign / reduction_ratio * kDegToRad; + velocity_to_command_velocity_coefficient_ = sign * reduction_ratio * kRadToDeg * 100.0; + + status_current_to_torque_coefficient_ = + sign * (current_max / kRawCurrentMax) * torque_constant * reduction_ratio; + torque_to_command_current_coefficient_ = 1 / status_current_to_torque_coefficient_; + + *max_torque_output_ = max_torque(); + } + + void store_status(std::span can_data) { + const CanPacket8 can_packet{can_data}; + const struct [[gnu::packed]] { + uint8_t command; + uint8_t placeholder[7]; + } feedback alignas(CanPacket8) = std::bit_cast(can_packet); + + // Exclude non-motor status messages + if ((feedback.command & 0xF0) != 0x80) + can_packet_.store(can_packet, std::memory_order::relaxed); } void update_status() { - librmcs::device::LkMotor::update_status(); - *angle_ = angle(); - *velocity_ = velocity(); - *torque_ = torque(); - *temperature_ = temperature(); + const struct [[gnu::packed]] { + uint8_t command; + int8_t temperature; + int16_t current; + int16_t velocity; + uint16_t encoder; + } feedback alignas(CanPacket8) = + std::bit_cast(can_packet_.load(std::memory_order::relaxed)); + + // Temperature unit: celsius + temperature_ = static_cast(feedback.temperature); + + // Angle unit: rad + const auto raw_angle = feedback.encoder; + auto calibrated_raw_angle = feedback.encoder - encoder_zero_point_; + if (calibrated_raw_angle < 0) + calibrated_raw_angle += raw_angle_max_; + if (!multi_turn_angle_enabled_) { + angle_ = status_angle_to_angle_coefficient_ * static_cast(calibrated_raw_angle); + if (angle_ < 0) + angle_ += 2 * std::numbers::pi; + } else { + // Calculates the minimal difference between two angles and normalizes it to the range + // (-raw_angle_max_/2, raw_angle_max_/2]. + // This implementation leverages bitwise operations for efficiency, which is valid only + // when raw_angle_max_ is a power of 2. + auto diff = (calibrated_raw_angle - multi_turn_encoder_count_) & (raw_angle_max_ - 1); + if (diff > (raw_angle_max_ >> 1)) + diff -= raw_angle_max_; + + multi_turn_encoder_count_ += diff; + angle_ = + status_angle_to_angle_coefficient_ * static_cast(multi_turn_encoder_count_); + } + last_raw_angle_ = raw_angle; + + // Velocity unit: rad/s + velocity_ = + status_velocity_to_velocity_coefficient_ * static_cast(feedback.velocity); + + // Torque unit: N*m + torque_ = status_current_to_torque_coefficient_ * static_cast(feedback.current); + + *angle_output_ = angle(); + *velocity_output_ = velocity(); + *torque_output_ = torque(); + *temperature_output_ = temperature(); + } + + int64_t calibrate_zero_point() { + multi_turn_encoder_count_ = 0; + encoder_zero_point_ = last_raw_angle_; + RCLCPP_INFO(rclcpp::get_logger("awa"), "calibrate: %d", encoder_zero_point_); + return encoder_zero_point_; + } + + double angle() const { return angle_; } + double velocity() const { return velocity_; } + double torque() const { return torque_; } + double max_torque() const { return max_torque_; } + double temperature() const { return temperature_; } + + /// @brief Switch the motor from the startup state (default state after power-on) to the + /// shutdown state, clearing the motor's rotation count and previously received control + /// commands. The LED changes from steady on to slow flashing. At this time, the motor can still + /// respond to control commands but will not execute actions. + constexpr static CanPacket8 generate_shutdown_command() { + const struct [[gnu::packed]] { + uint8_t id; + uint8_t placeholder[7]{}; + } command alignas(CanPacket8){.id = 0x80}; + return std::bit_cast(command); + } + + /// @brief Switch the motor from the shutdown state to the startup state. The LED changes from + /// slow flashing to steady on. At this point, sending control commands can control motor + /// actions. + constexpr static CanPacket8 generate_startup_command() { + const struct [[gnu::packed]] { + uint8_t id = 0x88; + uint8_t placeholder[7]{}; + } command alignas(CanPacket8){}; + return std::bit_cast(command); + } + + /// @brief Disable the motor, but do not clear the motor's running state. Sending control + /// commands again can control the motor actions. + constexpr static CanPacket8 generate_disable_command() { + // Note: instead of sending a real disable message here, a torque control message with + // torque set to 0 is sent, because the disable message does not cause the motor to feedback + // its status. + const struct [[gnu::packed]] { + uint8_t id = 0xA1; + uint8_t placeholder0[3]{}; + int16_t current = 0; + uint8_t placeholder1[2]{}; + } command alignas(CanPacket8){}; + return std::bit_cast(command); + } + + /// @brief This command reads the current motor's temperature, motor torque current (MF, MG) / + /// motor output power (MS), speed, and encoder position. + constexpr static CanPacket8 generate_status_request() { + const struct [[gnu::packed]] { + uint8_t id = 0x9C; + uint8_t placeholder[7]{}; + } request alignas(CanPacket8){}; + return std::bit_cast(request); + } + + /// @brief The host sends this command to control the motor's torque current output. + /// @note After receiving the command, the motor responds to the host. The motor's response data + /// is the same as the `generate_status_request` command (only the command byte 0 is different, + /// here it is 0xA1). + CanPacket8 generate_torque_command(double control_torque) const { + if (std::isnan(control_torque)) + return generate_disable_command(); + + /// @param current The value range is -2048~2048, corresponding to the actual torque current + /// range of MF motor -16.5A~16.5A, and the actual torque current range of MG motor + /// -33A~33A. The bus current and the motor's actual torque vary depending on the motor + /// type. + const struct [[gnu::packed]] { + uint8_t id = 0xA1; + uint8_t placeholder0[3]{}; + int16_t current; + uint8_t placeholder1[2]{}; + } command alignas(CanPacket8){.current = to_command_current(control_torque)}; + + return std::bit_cast(command); + } + + CanPacket8 generate_torque_command() const { return generate_torque_command(control_torque()); } + + /// @brief The host sends this command to control the motor's speed, along with a torque limit. + /// @note After receiving the command, the motor responds to the host. The motor's response data + /// is the same as the `generate_status_request` command (only the command byte 0 is + /// different, here it is 0xA2/0xAD). + CanPacket8 + generate_velocity_command(double control_velocity, double torque_limit = kNan) const { + if (std::isnan(control_velocity)) + return generate_disable_command(); + + /// @param torque_limit int16_t type, value range -2048~2048, corresponding to the actual + /// torque current range of MF motor -16.5A~16.5A, and MG motor -33.0A~33.0A. The bus + /// current and motor's actual torque vary depending on the motor type. + /// @param velocity int32_t type, corresponding to the actual speed as 0.01 dps/LSB; + struct [[gnu::packed]] { + uint8_t id = 0xA2; + uint8_t placeholder{}; + int16_t current_limit = 0; + int32_t velocity; + } command alignas(CanPacket8){.velocity = to_command_velocity(control_velocity)}; + + if (!std::isnan(torque_limit)) { + command.current_limit = to_command_current(torque_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_velocity_command() const { + return generate_velocity_command(control_velocity()); + } + + /// @brief The host sends this command to control the motor's position (multi-turn angle). + /// @note After receiving the command, the motor responds to the host. The motor's response data + /// is the same as the `generate_status_request` command (only the command byte 0 is + /// different, here it is 0xA3/0xA4). + CanPacket8 generate_angle_command(double control_angle, double velocity_limit = kNan) const { + if (std::isnan(control_angle)) + return generate_disable_command(); + + /// @param angle The actual position corresponds to 0.01 deg/LSB, meaning 36000 + /// represents 360 degrees, and the motor's rotation direction is determined by the + /// difference between the target position and the current position. + /// @param velocity The maximum speed limit for motor rotation, corresponding to an actual + /// speed of 1 dps/LSB, meaning 360 represents 360 dps. + struct [[gnu::packed]] { + uint8_t id = 0xA3; + uint8_t placeholder{}; + uint16_t velocity_limit = 0; + int32_t angle; + } command alignas(CanPacket8){.angle = to_absolute_command_angle(control_angle)}; + + if (!std::isnan(velocity_limit)) { + command.id = 0xA4; + + velocity_limit = + velocity_to_command_velocity_coefficient_ * (1.0 / 100.0) * velocity_limit; + velocity_limit = std::round( + std::clamp( + velocity_limit, std::numeric_limits::min(), + std::numeric_limits::max())); + command.velocity_limit = static_cast(velocity_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_angle_command() const { return generate_angle_command(control_angle()); } + + CanPacket8 generate_angle_shift_command( + double control_shift_angle, double velocity_limit = kNan) const { + if (std::isnan(control_shift_angle)) + return generate_disable_command(); + + /// @param angle The actual position corresponds to 0.01 deg/LSB, meaning 36000 + /// represents 360 degrees, and the motor direction of rotation is determined by + /// the sign of this parameter. + /// @param velocity_limit The maximum speed limit for motor rotation, corresponding to an + /// actual speed of 1 dps/LSB, meaning 360 represents 360 dps. + struct [[gnu::packed]] { + uint8_t id = 0xA7; + uint8_t placeholder{}; + uint16_t velocity_limit = 0; + int32_t angle; + } command alignas(CanPacket8){.angle = to_command_angle(control_shift_angle)}; + + if (!std::isnan(velocity_limit)) { + command.id = 0xA8; + + velocity_limit = + velocity_to_command_velocity_coefficient_ * (1.0 / 100.0) * velocity_limit; + velocity_limit = std::round( + std::clamp( + velocity_limit, std::numeric_limits::min(), + std::numeric_limits::max())); + command.velocity_limit = static_cast(velocity_limit); + } + + return std::bit_cast(command); + } + + CanPacket8 generate_angle_shift_command() const { + return generate_angle_shift_command(control_angle_shift()); + } + + CanPacket8 generate_command() { + if (first_generate_auto_command_) [[unlikely]] { + first_generate_auto_command_ = false; + if (!control_angle_shift_.ready() && !control_angle_.ready() + && !control_velocity_.ready() && !control_torque_.ready()) + throw std::runtime_error{"[LkMotor] No manipulating available!"}; + + if (!control_angle_shift_.ready()) + control_angle_shift_.bind_directly(kNan); + if (!control_angle_.ready()) + control_angle_.bind_directly(kNan); + if (!control_velocity_.ready()) + control_velocity_.bind_directly(kNan); + if (!control_torque_.ready()) + control_torque_.bind_directly(kNan); + } + + if (!std::isnan(control_angle_shift())) + return generate_angle_shift_command(control_angle_shift(), control_velocity()); + if (!std::isnan(control_angle())) + return generate_angle_command(control_angle(), control_velocity()); + if (!std::isnan(control_velocity())) + return generate_velocity_command(control_velocity(), control_torque()); + return generate_torque_command(control_torque()); } double control_torque() const { @@ -80,56 +440,82 @@ class LkMotor : public librmcs::device::LkMotor { return std::numeric_limits::quiet_NaN(); } - using librmcs::device::LkMotor::generate_torque_command; - uint64_t generate_torque_command() { return generate_torque_command(control_torque()); } - - using librmcs::device::LkMotor::generate_velocity_command; - uint64_t generate_velocity_command() { return generate_velocity_command(control_velocity()); } +private: + int16_t to_command_current(double torque) const { + double current = torque_to_command_current_coefficient_ * torque; + current = std::round(std::clamp(current, -kRawCurrentMax, kRawCurrentMax)); + return static_cast(current); + } - using librmcs::device::LkMotor::generate_angle_command; - uint64_t generate_angle_command() { return generate_angle_command(control_angle()); } + int32_t to_command_velocity(double velocity) const { + velocity = velocity_to_command_velocity_coefficient_ * velocity; + velocity = std::round( + std::clamp( + velocity, std::numeric_limits::min(), + std::numeric_limits::max())); + return static_cast(velocity); + } - using librmcs::device::LkMotor::generate_angle_shift_command; - uint64_t generate_angle_shift_command() { - return generate_angle_shift_command(control_angle_shift()); + int32_t to_command_angle(double angle) const { + angle = angle_to_command_angle_coefficient_ * angle; + angle = std::round( + std::clamp( + angle, std::numeric_limits::min(), std::numeric_limits::max())); + return static_cast(angle); } - uint64_t generate_command() { - if (first_generate_auto_command_) [[unlikely]] { - first_generate_auto_command_ = false; - if (!control_angle_shift_.ready() && !control_angle_.ready() - && !control_velocity_.ready() && !control_torque_.ready()) - throw std::runtime_error{"[LkMotor] No manipulating available!"}; - else { - if (!control_angle_shift_.ready()) - control_angle_shift_.bind_directly(nan_); - if (!control_angle_.ready()) - control_angle_.bind_directly(nan_); - if (!control_velocity_.ready()) - control_velocity_.bind_directly(nan_); - if (!control_torque_.ready()) - control_torque_.bind_directly(nan_); - } - } + int32_t to_absolute_command_angle(double angle) const { + angle = angle_to_command_angle_coefficient_ * angle; + angle -= std::abs(angle_to_command_angle_coefficient_) + * (((raw_angle_max_ - static_cast(encoder_zero_point_)) / raw_angle_max_) * 2 + * std::numbers::pi); + angle = std::round( + std::clamp( + angle, std::numeric_limits::min(), std::numeric_limits::max())); - if (!std::isnan(control_angle_shift())) - return generate_angle_shift_command(control_angle_shift(), control_velocity()); - else if (!std::isnan(control_angle())) - return generate_angle_command(control_angle(), control_velocity()); - else if (!std::isnan(control_velocity())) - return generate_velocity_command(control_velocity(), control_torque()); - else - return generate_torque_command(control_torque()); + return static_cast(angle); } -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + // Limits + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + + static constexpr int kRawCurrentMax = 2048; + int raw_angle_max_; + + // Constants + static constexpr double kDegToRad = std::numbers::pi / 180; + static constexpr double kRadToDeg = 180 / std::numbers::pi; + + bool multi_turn_angle_enabled_; + int encoder_zero_point_; + + // Coefficients + double status_angle_to_angle_coefficient_; + double angle_to_command_angle_coefficient_; + + double status_velocity_to_velocity_coefficient_; + double velocity_to_command_velocity_coefficient_; + + double status_current_to_torque_coefficient_; + double torque_to_command_current_coefficient_; + + // Status + std::atomic can_packet_; + + int64_t multi_turn_encoder_count_ = 0; + int last_raw_angle_ = 0; + + double angle_; + double torque_; + double velocity_; + double max_torque_; + double temperature_; - rmcs_executor::Component::OutputInterface angle_; - rmcs_executor::Component::OutputInterface velocity_; - rmcs_executor::Component::OutputInterface torque_; - rmcs_executor::Component::OutputInterface temperature_; - rmcs_executor::Component::OutputInterface max_torque_; + rmcs_executor::Component::OutputInterface angle_output_; + rmcs_executor::Component::OutputInterface velocity_output_; + rmcs_executor::Component::OutputInterface torque_output_; + rmcs_executor::Component::OutputInterface temperature_output_; + rmcs_executor::Component::OutputInterface max_torque_output_; rmcs_executor::Component::InputInterface control_torque_; rmcs_executor::Component::InputInterface control_velocity_; diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpp new file mode 100644 index 00000000..5a3f5174 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/as_byte_span.hpp @@ -0,0 +1,7 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_utility {} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp new file mode 100644 index 00000000..517fab9c --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/endian_promise.hpp @@ -0,0 +1,194 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace rmcs_utility { + +template +requires(std::is_integral_v || std::is_floating_point_v) +[[nodiscard]] inline T swap_endian(const T& value) noexcept { + static_assert( + sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8, + "Endian swap is only defined for 2, 4, and 8-byte types"); + + T result; + + const auto* value_bytes = reinterpret_cast(&value); + auto* result_bytes = reinterpret_cast(&result); + + if constexpr (sizeof(T) == 8) { // 64-bit + result_bytes[0] = value_bytes[7]; + result_bytes[1] = value_bytes[6]; + result_bytes[2] = value_bytes[5]; + result_bytes[3] = value_bytes[4]; + result_bytes[4] = value_bytes[3]; + result_bytes[5] = value_bytes[2]; + result_bytes[6] = value_bytes[1]; + result_bytes[7] = value_bytes[0]; + } else if constexpr (sizeof(T) == 4) { // 32-bit + result_bytes[0] = value_bytes[3]; + result_bytes[1] = value_bytes[2]; + result_bytes[2] = value_bytes[1]; + result_bytes[3] = value_bytes[0]; + } else if constexpr (sizeof(T) == 2) { // 16-bit + result_bytes[0] = value_bytes[1]; + result_bytes[1] = value_bytes[0]; + } else { + return 0; + } + + return result; +} + +template +requires(std::is_integral_v || std::is_floating_point_v) +struct [[gnu::packed]] EndianContainer final { + T value_buffer; + + [[nodiscard]] static T transform(const T& value) noexcept { + if constexpr (std::endian::native == target_endian) { + return value; + } else { + return swap_endian(value); + } + } + + EndianContainer() = default; + + // Storage in + EndianContainer(const T& value) noexcept // NOLINT(google-explicit-constructor) + : value_buffer(transform(value)) {} + + template + explicit EndianContainer(U const& value) noexcept + : value_buffer(transform(T(value))) {} + + // Storage out + template + operator U() const noexcept { // NOLINT(google-explicit-constructor) + return U(transform(value_buffer)); + } + + operator T() const noexcept { // NOLINT(google-explicit-constructor) + return transform(value_buffer); + } + + template + bool operator==(U const& o) const noexcept { + return U(*this) == o; + } + template + bool operator!=(U const& o) const noexcept { + return U(*this) != o; + } + + // Arithmetic assignment operators + EndianContainer& operator++() noexcept /* prefix */ { + *this = T(*this) + T(1); + return *this; + } + EndianContainer operator++(int) noexcept /* suffix */ { + EndianContainer t(*this); + *this = T(*this) + T(1); + return t; + } + EndianContainer& operator--() noexcept /* prefix */ { + *this = T(*this) - T(1); + return *this; + } + EndianContainer operator--(int) noexcept /* suffix */ { + EndianContainer t(*this); + *this = T(*this) - T(1); + return t; + } + + // Compound assignment operators + EndianContainer& operator+=(const T& value) noexcept { + *this = T(*this) + value; + return *this; + } + EndianContainer& operator-=(const T& value) noexcept { + *this = T(*this) - value; + return *this; + } + EndianContainer& operator*=(const T& value) noexcept { + *this = T(*this) * value; + return *this; + } + EndianContainer& operator/=(const T& value) noexcept { + *this = T(*this) / value; + return *this; + } + EndianContainer& operator%=(const T& value) noexcept { + *this = T(*this) % value; + return *this; + } + EndianContainer& operator&=(const T& value) noexcept { + *this = T(*this) & value; + return *this; + } + EndianContainer& operator|=(const T& value) noexcept { + *this = T(*this) | value; + return *this; + } + EndianContainer& operator^=(const T& value) noexcept { + *this = T(*this) ^ value; + return *this; + } + EndianContainer& operator<<=(const T& value) noexcept { + *this = T(T(*this) << value); + return *this; + } + EndianContainer& operator>>=(const T& value) noexcept { + *this = T(T(*this) >> value); + return *this; + } + friend std::ostream& operator<<(std::ostream& out, const EndianContainer value) { + out << T(value); + return out; + } + friend std::istream& operator>>(std::istream& in, EndianContainer& value) { + T val; + in >> val; + value = val; + return in; + } +}; + +template +requires(std::is_integral_v || std::is_floating_point_v) +// NOLINTNEXTLINE(readability-identifier-naming) +using little_endian_t = EndianContainer; + +template +requires(std::is_integral_v || std::is_floating_point_v) +// NOLINTNEXTLINE(readability-identifier-naming) +using big_endian_t = EndianContainer; + +using le_int16_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_int32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_int64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using le_uint16_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_uint32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_uint64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using le_float32_t = little_endian_t; // NOLINT(readability-identifier-naming) +using le_float64_t = little_endian_t; // NOLINT(readability-identifier-naming) + +using be_int16_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_int32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_int64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +using be_uint16_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_uint32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_uint64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +using be_float32_t = big_endian_t; // NOLINT(readability-identifier-naming) +using be_float64_t = big_endian_t; // NOLINT(readability-identifier-naming) + +} // namespace rmcs_utility \ No newline at end of file From 535fa82ff9fc804b7e0403cb442172d437195b9a Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 11 Mar 2026 02:02:44 +0800 Subject: [PATCH 33/45] Add device: pwm servo and Modify force sensor --- .../src/hardware/device/force_sensor.hpp | 17 +++++---- .../src/hardware/device/pwm_servo.hpp | 35 +++++++++++++++++++ 2 files changed, 46 insertions(+), 6 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp index 53fccde9..c6a1b769 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -1,5 +1,6 @@ #pragma once +#include "hardware/device/can_packet.hpp" #include #include #include @@ -18,17 +19,19 @@ class ForceSensor { status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_); } - void store_status(const uint64_t can_data) { - force_sensor_status_.store(std::bit_cast(can_data), std::memory_order::relaxed); + void store_status(std::span can_data) { + can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); } void update_status() { auto status = force_sensor_status_.load(std::memory_order::relaxed); - uint32_t ch1_weight = static_cast(status.ch1_0) << 24 | static_cast(status.ch1_1) << 16 - | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); - uint32_t ch2_weight = static_cast(status.ch2_0) << 24 | static_cast(status.ch2_1) << 16 - | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); + uint32_t ch1_weight = + static_cast(status.ch1_0) << 24 | static_cast(status.ch1_1) << 16 + | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); + uint32_t ch2_weight = + static_cast(status.ch2_0) << 24 | static_cast(status.ch2_1) << 16 + | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); *weight_ch1_ = std::bit_cast(ch1_weight); *weight_ch2_ = std::bit_cast(ch2_weight); @@ -50,6 +53,8 @@ class ForceSensor { uint8_t ch2_3; }; std::atomic force_sensor_status_{}; + + std::atomic can_data_; static_assert(decltype(force_sensor_status_)::is_always_lock_free); rmcs_executor::Component::OutputInterface weight_ch1_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp new file mode 100644 index 00000000..cdeddc78 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/pwm_servo.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +class PWMServo { +public: + PWMServo( + const std::string& name, rmcs_executor::Component& command_component, double pwm_cycle_time, + double min_position_high_time, double max_position_high_time) + : duty_offset_( + (min_position_high_time / pwm_cycle_time) * std::numeric_limits::max()) + , duty_scale_( + ((max_position_high_time - min_position_high_time) / pwm_cycle_time) + * std::numeric_limits::max()) { + command_component.register_input(name + "/value", value_); + } + constexpr uint16_t generate_duty_cycle() const { + double value = std::clamp(*value_, 0.0, 1.0); + double duty = duty_offset_ + (value * duty_scale_); + return static_cast(std::round(duty)); + } + +private: + const double duty_offset_; + const double duty_scale_; + + rmcs_executor::Component::InputInterface value_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file From 13b5746b27970c77a15e4eeea8dd4eb86d72d5c8 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 11 Mar 2026 02:03:31 +0800 Subject: [PATCH 34/45] Add new hardware for librmcs v3.1.0 and servo test --- rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml | 14 ++ rmcs_ws/src/rmcs_core/plugins.xml | 6 + .../src/controller/dart/pwm_test.cpp | 28 ++++ .../src/hardware/catapult_dart_librmcs_v3.cpp | 142 ++++++++++++++++++ 4 files changed, 190 insertions(+) create mode 100644 rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml b/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml new file mode 100644 index 00000000..2a1c3b81 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml @@ -0,0 +1,14 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDartforlibrmcsv3 -> catapult_dart_hardware + - rmcs_core::controller::test::PWMTest -> pwm_test_controller + +catapult_dart_hardware: + ros__parameters: + serial_filter: "" + +pwm_test_controller: + ros__parameters: + set_value: 0.5 \ 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 77acf075..e852e681 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -125,4 +125,10 @@ Test plugin. + + Test plugin. + + + Test plugin. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp new file mode 100644 index 00000000..5b6ffea5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp @@ -0,0 +1,28 @@ + +#include +#include + +namespace rmcs_core::controller::test { +class PWMTest + : public rmcs_executor::Component + , public rclcpp::Node { +public: + PWMTest() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + set_value_ = get_parameter("set_value").as_double(); + register_output("/dart/trigger_servo/value", value_); + } + + void update() override { *value_ = set_value_; } + +private: + double set_value_ = 0.0; + OutputInterface value_; +}; +} // namespace rmcs_core::controller::test + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::test::PWMTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp new file mode 100644 index 00000000..b276a96b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp @@ -0,0 +1,142 @@ +#include +#include + +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/force_sensor.hpp" +#include "hardware/device/pwm_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +namespace rmcs_core::hardware { + +class CatapultDartforlibrmcsv3 + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartforlibrmcsv3() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} + , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} + , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} + , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} + , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} + , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} + , force_sensor_{*this} { + + left_belt_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + + right_belt_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + + yaw_control_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + + pitch_control_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + + screw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + } + + CatapultDartforlibrmcsv3(const CatapultDartforlibrmcsv3&) = delete; + CatapultDartforlibrmcsv3& operator=(const CatapultDartforlibrmcsv3&) = delete; + CatapultDartforlibrmcsv3(CatapultDartforlibrmcsv3&&) = delete; + CatapultDartforlibrmcsv3& operator=(CatapultDartforlibrmcsv3&&) = delete; + + ~CatapultDartforlibrmcsv3() override = default; + + void update() override { force_sensor_.update_status(); } + + void command_update() { + auto board = start_transmit(); + board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + + board.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + pitch_control_motor_.generate_command(), + yaw_control_motor_.generate_command(), + screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + board.can2_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + left_belt_motor_.generate_command(), + right_belt_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }); + + if (pub_time_count_++ > 100) { + board.can2_transmit({ + .can_id = 0x301, + .can_data = device::CanPacket8{device::ForceSensor::generate_command()}.as_bytes(), + }); + pub_time_count_ = 0; + } + } + +protected: + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + + auto can_id = data.can_id; + if (can_id == 0x201) { + pitch_control_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + yaw_control_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + screw_motor_.store_status(data.can_data); + } else if (can_id == 0x205) { + left_belt_motor_.store_status(data.can_data); + } else if (can_id == 0x206) { + right_belt_motor_.store_status(data.can_data); + } else if (can_id == 0x302) { + force_sensor_.store_status(data.can_data); + } + } + +private: + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartforlibrmcsv3& dart) + : dart_(dart) {} + + void update() override { dart_.command_update(); } + + private: + CatapultDartforlibrmcsv3& dart_; + }; + std::shared_ptr dart_command_; + + rclcpp::Logger logger_; + + device::DjiMotor left_belt_motor_, right_belt_motor_; + device::DjiMotor yaw_control_motor_, pitch_control_motor_; + device::DjiMotor screw_motor_; + device::PWMServo trigger_servo_; + device::ForceSensor force_sensor_; + int pub_time_count_ = 0; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartforlibrmcsv3, rmcs_executor::Component) \ No newline at end of file From 61425caa1d6a443074748e3dbe7d46d60aff6753 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 11 Mar 2026 02:05:22 +0800 Subject: [PATCH 35/45] Clear codes and run success without real motor --- .../rmcs_core/src/hardware/catapult_dart.cpp | 1106 +++++++++-------- .../hardware/dart_filling_test_hardware.cpp | 642 +++++----- .../src/hardware/force_sensor_receive.cpp | 37 - .../src/hardware/force_sensor_setting.cpp | 99 -- .../src/hardware/force_sensor_test.cpp | 134 -- rmcs_ws/src/rmcs_core/src/hardware/hero.cpp | 917 +++++++------- .../src/rmcs_core/src/hardware/infantry.cpp | 627 +++++----- .../rmcs_core/src/hardware/steering-hero.cpp | 1045 ++++++++-------- .../src/hardware/steering-infantry.cpp | 885 +++++++------ .../src/hardware/tunnel_infantry.cpp | 613 ++++----- 10 files changed, 2941 insertions(+), 3164 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 528aa87b..2dfcace0 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,547 +1,559 @@ -#include "librmcs/client/cboard.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/force_sensor_runtime.hpp" -#include "hardware/device/trigger_servo.hpp" -#include -#include -#include -#include -#include -#include -#include "filter/low_pass_filter.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class CatapultDart - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { -public: - CatapultDart() - : 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()) - , pitch_angle_filter_(20.0, 1000.0) - , pitch_velocity_filter_(20.0, 1000.0) - , yaw_velocity_filter_(20.0, 1000.0) - , yaw_filter_(5.0, 1000.0) - , pitch_filter_(10.0, 1000.0) - , roll_filter_(10.0, 1000.0) - , dart_command_(create_partner_component(get_component_name() + "_command", *this)) - , dr16_(*this) - , imu_(1000, 0.2, 0.0) - , pitch_motor_( - *this, *dart_command_, "/dart/pitch_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) - , yaw_motor_( - *this, *dart_command_, "/dart/yaw_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) - , force_control_motor_( - *this, *dart_command_, "/dart/force_control_motor", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) - , drive_belt_motor_( - {*this, *dart_command_, "/dart/drive_belt/left", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, - {*this, *dart_command_, "/dart/drive_belt/right", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(19.) - .set_reversed()}) - , force_sensor_(*this) - , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) - , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) - , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) - , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); - register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - - first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); - final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - - setup_imu_coordinate_mapping(); - imu_sampler_initialize(); - tf_broadcaster_ = std::make_unique(*this); - start_time_ = std::chrono::steady_clock::now(); - - trigger_calibrate_subscription_ = create_subscription( - "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); - }); - limiting_calibrate_subscription_ = create_subscription( - "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); - }); - lifting_left_calibrate_subscription_ = create_subscription( - "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr); - }); - lifting_right_calibrate_subscription_ = create_subscription( - "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr); - }); - - 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; - }; - - last_read_left_time_ = this->now(); - last_read_right_time_ = this->now(); - } - - ~CatapultDart() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { - dr16_.update_status(); - pitch_motor_.update_status(); - yaw_motor_.update_status(); - drive_belt_motor_[0].update_status(); - drive_belt_motor_[1].update_status(); - force_control_motor_.update_status(); - force_sensor_.update_status(); - - imu_.update_status(); - processImuData(); - } - - void command_update() { - uint16_t can_commands[4]; - - if (pub_time_count_++ > 100) { - transmit_buffer_.add_can1_transmission( - 0x301, std::bit_cast(force_sensor_.generate_command())); - pub_time_count_ = 0; - } - - can_commands[0] = pitch_motor_.generate_command(); - can_commands[1] = yaw_motor_.generate_command(); - can_commands[2] = force_control_motor_.generate_command(); - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - - can_commands[0] = drive_belt_motor_[0].generate_command(); - can_commands[1] = drive_belt_motor_[1].generate_command(); - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - - if (!trigger_servo_.calibrate_mode()) { - uint16_t current_target = trigger_servo_.get_target_angle(); - if (current_target != last_trigger_angle_) { - size_t uart_data_length; - std::unique_ptr command_buffer = - trigger_servo_.generate_runtime_command(uart_data_length); - const auto trigger_servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); - last_trigger_angle_ = current_target; - } - } - - if (!limiting_servo_.calibrate_mode()) { - uint16_t current_target = limiting_servo_.get_target_angle(); - if (current_target != last_limiting_angle_) { - size_t uart_data_length; - auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_limiting_angle_ = current_target; - } - } - - if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { - uint16_t current_target_left = lifting_left_.get_target_angle(); - uint16_t current_target_right = lifting_right_.get_target_angle(); - if (current_target_left != last_lifting_left_angle_) { - size_t uart_data_length; - uint16_t runtime_left = 0; - uint16_t runtime_right = 0; - std::unique_ptr command_buffer = - device::TriggerServo::generate_sync_run_command(uart_data_length, - LIFTING_LEFT_ID, LIFTING_RIGHT_ID, - current_target_left, current_target_right, - runtime_left, runtime_right); - const auto lifting_table_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); - last_lifting_left_angle_ = current_target_left; - } - } - - - auto now = this->now(); - - if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { - size_t uart_data_length; - auto command_buffer = lifting_left_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_read_left_time_ = now; - } - if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) { - if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { - size_t uart_data_length; - auto command_buffer = lifting_right_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_read_right_time_ = now; - } - } - - transmit_buffer_.trigger_transmission(); - } - - int pub_time_count_ = 0; - -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 == 0x302) { - force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x201) { - pitch_motor_.store_status(can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(can_data); - } else if (can_id == 0x203) { - force_control_motor_.store_status(can_data); - } else if (can_id == 0x205) { - drive_belt_motor_[0].store_status(can_data); - } else if (can_id == 0x206) { - drive_belt_motor_[1].store_status(can_data); - } - } - - void uart1_receive_callback(const std::byte* data, uint8_t length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&data](std::byte* storage) { *storage = *data++; }, length); - } - - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - std::string hex_string = bytes_to_hex_string(data, length); - RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, hex_string.c_str()); - - if (length < 3) { - RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); - return; - } - - uint8_t servo_id = static_cast(data[2]); - std::pair result{false, 0}; - - switch (servo_id) { - case TRIGGER_SERVO_ID: - result = trigger_servo_.calibrate_current_angle(logger_, data, length); - if (!result.first) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIMITING_SERVO_ID: - result = limiting_servo_.calibrate_current_angle(logger_, data, length); - if (!result.first) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIFTING_LEFT_ID: - result = lifting_left_.calibrate_current_angle(logger_, data, length); - if (result.first) { - *lifting_current_angle_left_ = result.second; - } else { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIFTING_RIGHT_ID: - result = lifting_right_.calibrate_current_angle(logger_, data, length); - if (result.first) { - *lifting_current_angle_right_ = result.second; - } else { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - break; - } - - } - - 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 { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - -private: - static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; - static constexpr uint8_t LIMITING_SERVO_ID = 0x02; - static constexpr uint8_t LIFTING_LEFT_ID = 0x03; - static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; - std::byte * trigger_servo_uart_data_ptr; - std::byte * limiting_servo_uart_data_ptr; - std::byte * lifting_left_uart_data_ptr; - std::byte * lifting_right_uart_data_ptr; - - void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ - , std_msgs::msg::Int32::UniquePtr msg - , std::byte* servo_uart_data_ptr) { - /* - 标定命令格式： - ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" - 替换data值就行 - */ - servo_.set_calibrate_mode(msg->data); - - std::unique_ptr command_buffer; - size_t command_length = 0; - if (msg->data == 0) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); - } else if (msg->data == 1) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); - } else if (msg->data == 2) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); - } else if (msg->data == 3) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); - } else if (msg->data == 4) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); - } else if (msg->data == 5) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); - } else if (msg->data == 6) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); - } - - servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - - std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); - RCLCPP_INFO( - this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, - hex_string.c_str()); - } - - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) { - return "[]"; - } - - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - - for (size_t i = 0; i < size; ++i) { - ss << std::setw(2) << static_cast(data[i]) << " "; - } - - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') { - result.pop_back(); - } - return result; - } // DEBUG TOOL - - void setup_imu_coordinate_mapping() { - imu_.set_coordinate_mapping( - [](double x, double y, double z) -> std::tuple { - return {x, -y, -z}; - }); - } - - void imu_sampler_initialize() { - start_time_ = std::chrono::steady_clock::now(); - yaw_drift_coefficient_ = 0.0; - } - - void processImuData() { - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); - - Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - - double roll = std::atan2(rotation_matrix(2,1), rotation_matrix(2,2)); - double pitch = std::asin(-rotation_matrix(2,0)); - double yaw = std::atan2(rotation_matrix(1,0), rotation_matrix(0,0)); - - double transformed_roll = -roll_filter_.update(roll); - double transformed_pitch = pitch_filter_.update(pitch); - double transformed_yaw = -yaw_filter_.update(yaw); - - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - yaw_samples_.push_back(transformed_yaw); - time_samples_.push_back(elapsed_seconds); - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = static_cast(yaw_samples_.size()); - - for (int i = 0; i < n; ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } - - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) { - yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; - } - } - - transformed_yaw -= ((yaw_drift_coefficient_ + 0.000512) * elapsed_seconds); - publishTfTransforms(transformed_roll, transformed_pitch, transformed_yaw); - - *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; - *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; - } - - void publishTfTransforms(double roll, double pitch, double yaw) { - auto now = this->get_clock()->now(); - - auto create_transform = [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; - - auto create_translation = [](double x, double y, double z) { - geometry_msgs::msg::Vector3 t; - t.x = x; t.y = y; t.z = z; - return t; - }; - - auto create_rotation = [](double x, double y, double z, double w) { - geometry_msgs::msg::Quaternion r; - r.x = x; r.y = y; r.z = z; r.w = w; - return r; - }; - - geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); - geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - - Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); - Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - - tf_broadcaster_->sendTransform(create_transform("base_link", "gimbal_center_link", - zero_trans, create_rotation(0, 0, 0, 1))); - tf_broadcaster_->sendTransform(create_transform("gimbal_center_link", "yaw_link", - zero_trans, create_rotation(yaw_quat.x(), yaw_quat.y(), - yaw_quat.z(), yaw_quat.w()))); - tf_broadcaster_->sendTransform(create_transform("yaw_link", "pitch_link", - pitch_trans, create_rotation(pitch_quat.x(), pitch_quat.y(), - pitch_quat.z(), pitch_quat.w()))); - tf_broadcaster_->sendTransform(create_transform("pitch_link", "odom_imu", - zero_trans, create_rotation(combined_quaternion.x(), combined_quaternion.y(), - combined_quaternion.z(), combined_quaternion.w()))); - tf_broadcaster_->sendTransform(create_transform("world", "base_link", - zero_trans, create_rotation(0, 0, 0, 1))); - } - - rclcpp::Logger logger_; - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - filter::LowPassFilter<1> yaw_filter_; - filter::LowPassFilter<1> pitch_filter_; - filter::LowPassFilter<1> roll_filter_; - std::chrono::steady_clock::time_point start_time_; - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDart& robot) : dart_(robot) {} - void update() override { dart_.command_update(); } - CatapultDart& dart_; - }; - - std::shared_ptr dart_command_; - double first_sample_spot_; - double final_sample_spot_; - OutputInterface tf_; - std::unique_ptr tf_broadcaster_; - device::Dr16 dr16_; - device::Bmi088 imu_; - device::DjiMotor pitch_motor_; - device::DjiMotor yaw_motor_; - device::DjiMotor force_control_motor_; - device::DjiMotor drive_belt_motor_[2]; - device::ForceSensorRuntime force_sensor_; - device::TriggerServo trigger_servo_; - device::TriggerServo limiting_servo_; - device::TriggerServo lifting_left_; - device::TriggerServo lifting_right_; - rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; - rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; - rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; - rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - OutputInterface catapult_pitch_angle_; - OutputInterface catapult_roll_angle_; - OutputInterface catapult_yaw_angle_; - OutputInterface lifting_current_angle_left_; - OutputInterface lifting_current_angle_right_; - double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_, time_samples_; - uint16_t last_trigger_angle_ = 0xFFFF; - uint16_t last_limiting_angle_ = 0xFFFF; - uint16_t last_lifting_left_angle_ = 0xFFFF; - rclcpp::Time last_read_left_time_; - rclcpp::Time last_read_right_time_; - const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -}; -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file +// #include "filter/low_pass_filter.hpp" +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/force_sensor_runtime.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include "librmcs/agent/c_board.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware { + +// class CatapultDart +// : public rmcs_executor::Component +// , public rclcpp::Node +// , public librmcs::agent::CBoard { +// public: +// CatapultDart() +// : Node{get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , +// librmcs::agent::CBoard{static_cast(get_parameter("usb_pid").as_int())} , +// logger_(get_logger()) , pitch_angle_filter_(20.0, 1000.0) , pitch_velocity_filter_(20.0, +// 1000.0) , yaw_velocity_filter_(20.0, 1000.0) , yaw_filter_(5.0, 1000.0) , +// pitch_filter_(10.0, 1000.0) , roll_filter_(10.0, 1000.0) , dart_command_( +// create_partner_component(get_component_name() + "_command", *this)) +// , dr16_(*this) +// , imu_(1000, 0.2, 0.0) +// , pitch_motor_( +// *this, *dart_command_, "/dart/pitch_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , yaw_motor_( +// *this, *dart_command_, "/dart/yaw_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , force_control_motor_( +// *this, *dart_command_, "/dart/force_control_motor", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) +// , drive_belt_motor_( +// {*this, *dart_command_, "/dart/drive_belt/left", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, +// {*this, *dart_command_, "/dart/drive_belt/right", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(19.) +// .set_reversed()}) +// , force_sensor_(*this) +// , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) +// , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) +// , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) +// , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { +// register_output("/tf", tf_); +// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); +// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); +// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); +// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); +// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); + +// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); +// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + +// setup_imu_coordinate_mapping(); +// imu_sampler_initialize(); +// tf_broadcaster_ = std::make_unique(*this); +// start_time_ = std::chrono::steady_clock::now(); + +// trigger_calibrate_subscription_ = create_subscription( +// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback( +// trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); +// }); +// limiting_calibrate_subscription_ = create_subscription( +// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) +// { +// trigger_servo_calibrate_subscription_callback( +// limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); +// }); +// lifting_left_calibrate_subscription_ = create_subscription( +// "/lifting_left/calibrate", rclcpp::QoS{0}, +// [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback( +// lifting_left_, std::move(msg), lifting_left_uart_data_ptr); +// }); +// lifting_right_calibrate_subscription_ = create_subscription( +// "/lifting_right/calibrate", rclcpp::QoS{0}, +// [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback( +// lifting_right_, std::move(msg), lifting_right_uart_data_ptr); +// }); + +// 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; +// }; + +// last_read_left_time_ = this->now(); +// last_read_right_time_ = this->now(); +// } + +// ~CatapultDart() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// dr16_.update_status(); +// pitch_motor_.update_status(); +// yaw_motor_.update_status(); +// drive_belt_motor_[0].update_status(); +// drive_belt_motor_[1].update_status(); +// force_control_motor_.update_status(); +// force_sensor_.update_status(); + +// imu_.update_status(); +// processImuData(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// if (pub_time_count_++ > 100) { +// transmit_buffer_.add_can1_transmission( +// 0x301, std::bit_cast(force_sensor_.generate_command())); +// pub_time_count_ = 0; +// } + +// can_commands[0] = pitch_motor_.generate_command(); +// can_commands[1] = yaw_motor_.generate_command(); +// can_commands[2] = force_control_motor_.generate_command(); +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// can_commands[0] = drive_belt_motor_[0].generate_command(); +// can_commands[1] = drive_belt_motor_[1].generate_command(); +// can_commands[2] = 0; +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); + +// if (!trigger_servo_.calibrate_mode()) { +// uint16_t current_target = trigger_servo_.get_target_angle(); +// if (current_target != last_trigger_angle_) { +// size_t uart_data_length; +// std::unique_ptr command_buffer = +// trigger_servo_.generate_runtime_command(uart_data_length); +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission( +// trigger_servo_uart_data_ptr, uart_data_length); +// last_trigger_angle_ = current_target; +// } +// } + +// if (!limiting_servo_.calibrate_mode()) { +// uint16_t current_target = limiting_servo_.get_target_angle(); +// if (current_target != last_limiting_angle_) { +// size_t uart_data_length; +// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_limiting_angle_ = current_target; +// } +// } + +// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { +// uint16_t current_target_left = lifting_left_.get_target_angle(); +// uint16_t current_target_right = lifting_right_.get_target_angle(); +// if (current_target_left != last_lifting_left_angle_) { +// size_t uart_data_length; +// uint16_t runtime_left = 0; +// uint16_t runtime_right = 0; +// std::unique_ptr command_buffer = +// device::TriggerServo::generate_sync_run_command( +// uart_data_length, LIFTING_LEFT_ID, LIFTING_RIGHT_ID, current_target_left, +// current_target_right, runtime_left, runtime_right); +// const auto lifting_table_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission( +// lifting_table_uart_data_ptr, uart_data_length); +// last_lifting_left_angle_ = current_target_left; +// } +// } + +// auto now = this->now(); + +// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { +// size_t uart_data_length; +// auto command_buffer = lifting_left_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_read_left_time_ = now; +// } +// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) +// { +// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { +// size_t uart_data_length; +// auto command_buffer = lifting_right_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, +// uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_read_right_time_ = now; +// } +// } + +// transmit_buffer_.trigger_transmission(); +// } + +// int pub_time_count_ = 0; + +// 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 == 0x302) { +// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x201) { +// pitch_motor_.store_status(can_data); +// } else if (can_id == 0x202) { +// yaw_motor_.store_status(can_data); +// } else if (can_id == 0x203) { +// force_control_motor_.store_status(can_data); +// } else if (can_id == 0x205) { +// drive_belt_motor_[0].store_status(can_data); +// } else if (can_id == 0x206) { +// drive_belt_motor_[1].store_status(can_data); +// } +// } + +// void uart1_receive_callback(const std::byte* data, uint8_t length) override { +// referee_ring_buffer_receive_.emplace_back_multi( +// [&data](std::byte* storage) { *storage = *data++; }, length); +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// std::string hex_string = bytes_to_hex_string(data, length); +// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, +// hex_string.c_str()); + +// if (length < 3) { +// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); +// return; +// } + +// uint8_t servo_id = static_cast(data[2]); +// std::pair result{false, 0}; + +// switch (servo_id) { +// case TRIGGER_SERVO_ID: +// result = trigger_servo_.calibrate_current_angle(logger_, data, length); +// if (!result.first) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIMITING_SERVO_ID: +// result = limiting_servo_.calibrate_current_angle(logger_, data, length); +// if (!result.first) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIFTING_LEFT_ID: +// result = lifting_left_.calibrate_current_angle(logger_, data, length); +// if (result.first) { +// *lifting_current_angle_left_ = result.second; +// } else { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIFTING_RIGHT_ID: +// result = lifting_right_.calibrate_current_angle(logger_, data, length); +// if (result.first) { +// *lifting_current_angle_right_ = result.second; +// } else { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); break; +// } +// } + +// 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 { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// private: +// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; +// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; +// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; +// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; +// std::byte* trigger_servo_uart_data_ptr; +// std::byte* limiting_servo_uart_data_ptr; +// std::byte* lifting_left_uart_data_ptr; +// std::byte* lifting_right_uart_data_ptr; + +// void trigger_servo_calibrate_subscription_callback( +// device::TriggerServo& servo_, std_msgs::msg::Int32::UniquePtr msg, +// std::byte* servo_uart_data_ptr) { +// /* +// 标定命令格式： +// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" +// 替换data值就行 +// */ +// servo_.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; +// if (msg->data == 0) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); + +// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); +// RCLCPP_INFO( +// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, +// hex_string.c_str()); +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) { +// return "[]"; +// } + +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); + +// for (size_t i = 0; i < size; ++i) { +// ss << std::setw(2) << static_cast(data[i]) << " "; +// } + +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') { +// result.pop_back(); +// } +// return result; +// } // DEBUG TOOL + +// void setup_imu_coordinate_mapping() { +// imu_.set_coordinate_mapping( +// [](double x, double y, double z) -> std::tuple { +// return {x, -y, -z}; +// }); +// } + +// void imu_sampler_initialize() { +// start_time_ = std::chrono::steady_clock::now(); +// yaw_drift_coefficient_ = 0.0; +// } + +// void processImuData() { +// auto current_time = std::chrono::steady_clock::now(); +// double elapsed_seconds = std::chrono::duration(current_time - +// start_time_).count(); + +// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); +// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); + +// double roll = std::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); +// double pitch = std::asin(-rotation_matrix(2, 0)); +// double yaw = std::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)); + +// double transformed_roll = -roll_filter_.update(roll); +// double transformed_pitch = pitch_filter_.update(pitch); +// double transformed_yaw = -yaw_filter_.update(yaw); + +// if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { +// yaw_samples_.push_back(transformed_yaw); +// time_samples_.push_back(elapsed_seconds); +// double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; +// double n = static_cast(yaw_samples_.size()); + +// for (int i = 0; i < n; ++i) { +// sum_x += time_samples_[i]; +// sum_y += yaw_samples_[i]; +// sum_xy += time_samples_[i] * yaw_samples_[i]; +// sum_xx += time_samples_[i] * time_samples_[i]; +// } + +// double denominator = n * sum_xx - sum_x * sum_x; +// if (std::abs(denominator) > 1e-10) { +// yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; +// } +// } + +// transformed_yaw -= ((yaw_drift_coefficient_ + 0.000512) * elapsed_seconds); +// publishTfTransforms(transformed_roll, transformed_pitch, transformed_yaw); + +// *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; +// *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; +// *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; +// } + +// void publishTfTransforms(double roll, double pitch, double yaw) { +// auto now = this->get_clock()->now(); + +// auto create_transform = [&](const std::string& parent, const std::string& child, +// const geometry_msgs::msg::Vector3& trans, +// const geometry_msgs::msg::Quaternion& rot) { +// geometry_msgs::msg::TransformStamped tf; +// tf.header.stamp = now; +// tf.header.frame_id = parent; +// tf.child_frame_id = child; +// tf.transform.translation = trans; +// tf.transform.rotation = rot; +// return tf; +// }; + +// auto create_translation = [](double x, double y, double z) { +// geometry_msgs::msg::Vector3 t; +// t.x = x; +// t.y = y; +// t.z = z; +// return t; +// }; + +// auto create_rotation = [](double x, double y, double z, double w) { +// geometry_msgs::msg::Quaternion r; +// r.x = x; +// r.y = y; +// r.z = z; +// r.w = w; +// return r; +// }; + +// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); +// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); + +// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); +// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); +// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); +// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; + +// tf_broadcaster_->sendTransform(create_transform( +// "base_link", "gimbal_center_link", zero_trans, create_rotation(0, 0, 0, 1))); +// tf_broadcaster_->sendTransform(create_transform( +// "gimbal_center_link", "yaw_link", zero_trans, +// create_rotation(yaw_quat.x(), yaw_quat.y(), yaw_quat.z(), yaw_quat.w()))); +// tf_broadcaster_->sendTransform(create_transform( +// "yaw_link", "pitch_link", pitch_trans, +// create_rotation(pitch_quat.x(), pitch_quat.y(), pitch_quat.z(), pitch_quat.w()))); +// tf_broadcaster_->sendTransform(create_transform( +// "pitch_link", "odom_imu", zero_trans, +// create_rotation( +// combined_quaternion.x(), combined_quaternion.y(), combined_quaternion.z(), +// combined_quaternion.w()))); +// tf_broadcaster_->sendTransform( +// create_transform("world", "base_link", zero_trans, create_rotation(0, 0, 0, 1))); +// } + +// rclcpp::Logger logger_; +// filter::LowPassFilter<1> pitch_angle_filter_; +// filter::LowPassFilter<1> pitch_velocity_filter_; +// filter::LowPassFilter<1> yaw_velocity_filter_; +// filter::LowPassFilter<1> yaw_filter_; +// filter::LowPassFilter<1> pitch_filter_; +// filter::LowPassFilter<1> roll_filter_; +// std::chrono::steady_clock::time_point start_time_; +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDart& robot) +// : dart_(robot) {} +// void update() override { dart_.command_update(); } +// CatapultDart& dart_; +// }; + +// std::shared_ptr dart_command_; +// double first_sample_spot_; +// double final_sample_spot_; +// OutputInterface tf_; +// std::unique_ptr tf_broadcaster_; +// device::Dr16 dr16_; +// device::Bmi088 imu_; +// device::DjiMotor pitch_motor_; +// device::DjiMotor yaw_motor_; +// device::DjiMotor force_control_motor_; +// device::DjiMotor drive_belt_motor_[2]; +// device::ForceSensorRuntime force_sensor_; +// device::TriggerServo trigger_servo_; +// device::TriggerServo limiting_servo_; +// device::TriggerServo lifting_left_; +// device::TriggerServo lifting_right_; +// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; +// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; +// OutputInterface referee_serial_; +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// OutputInterface catapult_pitch_angle_; +// OutputInterface catapult_roll_angle_; +// OutputInterface catapult_yaw_angle_; +// OutputInterface lifting_current_angle_left_; +// OutputInterface lifting_current_angle_right_; +// double yaw_drift_coefficient_ = 0.0; +// std::vector yaw_samples_, time_samples_; +// uint16_t last_trigger_angle_ = 0xFFFF; +// uint16_t last_limiting_angle_ = 0xFFFF; +// uint16_t last_lifting_left_angle_ = 0xFFFF; +// rclcpp::Time last_read_left_time_; +// rclcpp::Time last_read_right_time_; +// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); +// }; +// } // namespace rmcs_core::hardware + +// #include +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp index 5e1382bf..42e60b0c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp @@ -1,316 +1,326 @@ -#include "librmcs/client/cboard.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/trigger_servo.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class DartFillingTestHardware - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { -public: - DartFillingTestHardware() - : 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()) - , dart_command_(create_partner_component(get_component_name() + "_command", *this)) - , dr16_(*this) - , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) - , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) - , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) - , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); - register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - - trigger_calibrate_subscription_ = create_subscription( - "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); - }); - limiting_calibrate_subscription_ = create_subscription( - "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); - }); - lifting_left_calibrate_subscription_ = create_subscription( - "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), lifting_left_uart_data_ptr); - }); - lifting_right_calibrate_subscription_ = create_subscription( - "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), lifting_right_uart_data_ptr); - }); - - 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; - }; - - last_read_left_time_ = this->now(); - last_read_right_time_ = this->now(); - } - - ~DartFillingTestHardware() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { - dr16_.update_status(); - } - - void command_update() { - - if (!trigger_servo_.calibrate_mode()) { - uint16_t current_target = trigger_servo_.get_target_angle(); - if (current_target != last_trigger_angle_) { - size_t uart_data_length; - std::unique_ptr command_buffer = - trigger_servo_.generate_runtime_command(uart_data_length); - const auto trigger_servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); - last_trigger_angle_ = current_target; - } - } - - if (!limiting_servo_.calibrate_mode()) { - uint16_t current_target = limiting_servo_.get_target_angle(); - if (current_target != last_limiting_angle_) { - size_t uart_data_length; - auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_limiting_angle_ = current_target; - } - } - - if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { - uint16_t current_target_left = lifting_left_.get_target_angle(); - uint16_t current_target_right = lifting_right_.get_target_angle(); - if (current_target_left != last_lifting_left_angle_) { - size_t uart_data_length; - uint16_t runtime_left = 0; - uint16_t runtime_right = 0; - std::unique_ptr command_buffer = - device::TriggerServo::generate_sync_run_command(uart_data_length, - LIFTING_LEFT_ID, LIFTING_RIGHT_ID, - current_target_left, current_target_right, - runtime_left, runtime_right); - const auto lifting_table_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, uart_data_length); - last_lifting_left_angle_ = current_target_left; - } - } - - - auto now = this->now(); - - if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { - size_t uart_data_length; - auto command_buffer = lifting_left_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_read_left_time_ = now; - } - if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) { - if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { - size_t uart_data_length; - auto command_buffer = lifting_right_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); - transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); - last_read_right_time_ = now; - } - } - - transmit_buffer_.trigger_transmission(); - } - - int pub_time_count_ = 0; - -protected: - void uart1_receive_callback(const std::byte* data, uint8_t length) override { - referee_ring_buffer_receive_.emplace_back_multi( - [&data](std::byte* storage) { *storage = *data++; }, length); - } - - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - std::string hex_string = bytes_to_hex_string(data, length); - RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, hex_string.c_str()); - - if (length < 3) { - RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); - return; - } - - uint8_t servo_id = static_cast(data[2]); - std::pair result{false, 0}; - - switch (servo_id) { - case TRIGGER_SERVO_ID: - result = trigger_servo_.calibrate_current_angle(logger_, data, length); - if (!result.first) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIMITING_SERVO_ID: - result = limiting_servo_.calibrate_current_angle(logger_, data, length); - if (!result.first) { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIFTING_LEFT_ID: - result = lifting_left_.calibrate_current_angle(logger_, data, length); - if (result.first) { - *lifting_current_angle_left_ = result.second; - } else { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - case LIFTING_RIGHT_ID: - result = lifting_right_.calibrate_current_angle(logger_, data, length); - if (result.first) { - *lifting_current_angle_right_ = result.second; - } else { - RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - } - break; - default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); - break; - } - - } - - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); - } - -private: - static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; - static constexpr uint8_t LIMITING_SERVO_ID = 0x02; - static constexpr uint8_t LIFTING_LEFT_ID = 0x03; - static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; - std::byte * trigger_servo_uart_data_ptr; - std::byte * limiting_servo_uart_data_ptr; - std::byte * lifting_left_uart_data_ptr; - std::byte * lifting_right_uart_data_ptr; - - void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ - , std_msgs::msg::Int32::UniquePtr msg - , std::byte* servo_uart_data_ptr) { - /* - 标定命令格式： - ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" - 替换data值就行 - */ - servo_.set_calibrate_mode(msg->data); - - std::unique_ptr command_buffer; - size_t command_length = 0; - if (msg->data == 0) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); - } else if (msg->data == 1) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); - } else if (msg->data == 2) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); - } else if (msg->data == 3) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); - } else if (msg->data == 4) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); - } else if (msg->data == 5) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); - } else if (msg->data == 6) { - command_buffer = servo_.generate_calibrate_command( - device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); - } - - servo_uart_data_ptr = command_buffer.get(); - transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - - std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); - RCLCPP_INFO( - this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, - hex_string.c_str()); - } - - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) { - return "[]"; - } - - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - - for (size_t i = 0; i < size; ++i) { - ss << std::setw(2) << static_cast(data[i]) << " "; - } - - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') { - result.pop_back(); - } - return result; - } // DEBUG TOOL - - rclcpp::Logger logger_; - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(DartFillingTestHardware& robot) : dart_(robot) {} - void update() override { dart_.command_update(); } - DartFillingTestHardware& dart_; - }; - - std::shared_ptr dart_command_; - device::Dr16 dr16_; - device::TriggerServo trigger_servo_; - device::TriggerServo limiting_servo_; - device::TriggerServo lifting_left_; - device::TriggerServo lifting_right_; - rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; - rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; - rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; - rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - OutputInterface lifting_current_angle_left_; - OutputInterface lifting_current_angle_right_; - std::vector yaw_samples_, time_samples_; - uint16_t last_trigger_angle_ = 0xFFFF; - uint16_t last_limiting_angle_ = 0xFFFF; - uint16_t last_lifting_left_angle_ = 0xFFFF; - rclcpp::Time last_read_left_time_; - rclcpp::Time last_read_right_time_; - const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -}; -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DartFillingTestHardware, rmcs_executor::Component) \ No newline at end of file +// #include "librmcs/client/cboard.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::hardware { + +// class DartFillingTestHardware +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// DartFillingTestHardware() +// : 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()) +// , dart_command_(create_partner_component(get_component_name() + "_command", +// *this)) , dr16_(*this) , trigger_servo_(*dart_command_, "/dart/trigger_servo", +// TRIGGER_SERVO_ID) , limiting_servo_(*dart_command_, "/dart/limiting_servo", +// LIMITING_SERVO_ID) , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) +// , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { +// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); +// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); + +// trigger_calibrate_subscription_ = create_subscription( +// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), +// trigger_servo_uart_data_ptr); +// }); +// limiting_calibrate_subscription_ = create_subscription( +// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) +// { +// trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), +// limiting_servo_uart_data_ptr); +// }); +// lifting_left_calibrate_subscription_ = create_subscription( +// "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& +// msg) { +// trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), +// lifting_left_uart_data_ptr); +// }); +// lifting_right_calibrate_subscription_ = create_subscription( +// "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& +// msg) { +// trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), +// lifting_right_uart_data_ptr); +// }); + +// 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; +// }; + +// last_read_left_time_ = this->now(); +// last_read_right_time_ = this->now(); +// } + +// ~DartFillingTestHardware() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// dr16_.update_status(); +// } + +// void command_update() { + +// if (!trigger_servo_.calibrate_mode()) { +// uint16_t current_target = trigger_servo_.get_target_angle(); +// if (current_target != last_trigger_angle_) { +// size_t uart_data_length; +// std::unique_ptr command_buffer = +// trigger_servo_.generate_runtime_command(uart_data_length); +// const auto trigger_servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, +// uart_data_length); last_trigger_angle_ = current_target; +// } +// } + +// if (!limiting_servo_.calibrate_mode()) { +// uint16_t current_target = limiting_servo_.get_target_angle(); +// if (current_target != last_limiting_angle_) { +// size_t uart_data_length; +// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_limiting_angle_ = current_target; +// } +// } + +// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { +// uint16_t current_target_left = lifting_left_.get_target_angle(); +// uint16_t current_target_right = lifting_right_.get_target_angle(); +// if (current_target_left != last_lifting_left_angle_) { +// size_t uart_data_length; +// uint16_t runtime_left = 0; +// uint16_t runtime_right = 0; +// std::unique_ptr command_buffer = +// device::TriggerServo::generate_sync_run_command(uart_data_length, +// LIFTING_LEFT_ID, +// LIFTING_RIGHT_ID, +// current_target_left, +// current_target_right, +// runtime_left, runtime_right); +// const auto lifting_table_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, +// uart_data_length); last_lifting_left_angle_ = current_target_left; +// } +// } + +// auto now = this->now(); + +// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { +// size_t uart_data_length; +// auto command_buffer = lifting_left_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_read_left_time_ = now; +// } +// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) +// { +// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { +// size_t uart_data_length; +// auto command_buffer = lifting_right_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, +// uart_data_length); +// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); +// last_read_right_time_ = now; +// } +// } + +// transmit_buffer_.trigger_transmission(); +// } + +// int pub_time_count_ = 0; + +// protected: +// void uart1_receive_callback(const std::byte* data, uint8_t length) override { +// referee_ring_buffer_receive_.emplace_back_multi( +// [&data](std::byte* storage) { *storage = *data++; }, length); +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// std::string hex_string = bytes_to_hex_string(data, length); +// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, +// hex_string.c_str()); + +// if (length < 3) { +// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); +// return; +// } + +// uint8_t servo_id = static_cast(data[2]); +// std::pair result{false, 0}; + +// switch (servo_id) { +// case TRIGGER_SERVO_ID: +// result = trigger_servo_.calibrate_current_angle(logger_, data, length); +// if (!result.first) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIMITING_SERVO_ID: +// result = limiting_servo_.calibrate_current_angle(logger_, data, length); +// if (!result.first) { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIFTING_LEFT_ID: +// result = lifting_left_.calibrate_current_angle(logger_, data, length); +// if (result.first) { +// *lifting_current_angle_left_ = result.second; +// } else { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// case LIFTING_RIGHT_ID: +// result = lifting_right_.calibrate_current_angle(logger_, data, length); +// if (result.first) { +// *lifting_current_angle_right_ = result.second; +// } else { +// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// } +// break; +// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); +// break; +// } + +// } + +// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { +// dr16_.store_status(uart_data, uart_data_length); +// } + +// private: +// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; +// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; +// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; +// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; +// std::byte * trigger_servo_uart_data_ptr; +// std::byte * limiting_servo_uart_data_ptr; +// std::byte * lifting_left_uart_data_ptr; +// std::byte * lifting_right_uart_data_ptr; + +// void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ +// , std_msgs::msg::Int32::UniquePtr msg +// , std::byte* servo_uart_data_ptr) { +// /* +// 标定命令格式： +// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" +// 替换data值就行 +// */ +// servo_.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; +// if (msg->data == 0) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = servo_.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// servo_uart_data_ptr = command_buffer.get(); +// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); + +// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); +// RCLCPP_INFO( +// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, +// hex_string.c_str()); +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) { +// return "[]"; +// } + +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); + +// for (size_t i = 0; i < size; ++i) { +// ss << std::setw(2) << static_cast(data[i]) << " "; +// } + +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') { +// result.pop_back(); +// } +// return result; +// } // DEBUG TOOL + +// rclcpp::Logger logger_; +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(DartFillingTestHardware& robot) : dart_(robot) {} +// void update() override { dart_.command_update(); } +// DartFillingTestHardware& dart_; +// }; + +// std::shared_ptr dart_command_; +// device::Dr16 dr16_; +// device::TriggerServo trigger_servo_; +// device::TriggerServo limiting_servo_; +// device::TriggerServo lifting_left_; +// device::TriggerServo lifting_right_; +// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; +// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; +// OutputInterface referee_serial_; +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// OutputInterface lifting_current_angle_left_; +// OutputInterface lifting_current_angle_right_; +// std::vector yaw_samples_, time_samples_; +// uint16_t last_trigger_angle_ = 0xFFFF; +// uint16_t last_limiting_angle_ = 0xFFFF; +// uint16_t last_lifting_left_angle_ = 0xFFFF; +// rclcpp::Time last_read_left_time_; +// rclcpp::Time last_read_right_time_; +// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); +// }; +// } // namespace rmcs_core::hardware + +// #include +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DartFillingTestHardware, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp deleted file mode 100644 index 8b53eaf2..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_receive.cpp +++ /dev/null @@ -1,37 +0,0 @@ -#include -#include -#include -#include -namespace rmcs_core::hardware { -class ForceSensorReceive - : public rmcs_executor::Component - , public rclcpp::Node { -public: - ForceSensorReceive() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - } - - void update() override { - log_count_++; - if (log_count_ == 100) { - log_count_ = 0; - RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); - } - }; - -private: - rclcpp::Logger logger_; - - int log_count_ = 0; - - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorReceive, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp deleted file mode 100644 index b8a50d2d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_setting.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#include "hardware/device/force_sensor_runtime.hpp" -#include "librmcs/client/cboard.hpp" -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class ForceSensorSetting - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { - -public: - ForceSensorSetting() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} - , robot_command_(create_partner_component(get_component_name() + "_command", *this)) - , force_sensor_(*this) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) {} - - ~ForceSensorSetting() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { force_sensor_.update_status(); } - - void command_update() { - - count_++; - if (count_ == 100) { - count_ = 0; - transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); - } - - transmit_buffer_.trigger_transmission(); - } - - int count_ = 0; - -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 == 0x302) { - force_sensor_.store_status(can_data); - sensor_.store(std::bit_cast(can_data), std::memory_order_relaxed); - auto data = sensor_.load(std::memory_order_relaxed); - - RCLCPP_INFO(get_logger(), "ch1: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch1_0, data.ch1_1, data.ch1_2, data.ch1_3); - RCLCPP_INFO(get_logger(), "ch2: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch2_0, data.ch2_1, data.ch2_2, data.ch2_3); - } - } - - struct sensor_data { - uint8_t ch1_0; - uint8_t ch1_1; - uint8_t ch1_2; - uint8_t ch1_3; - uint8_t ch2_0; - uint8_t ch2_1; - uint8_t ch2_2; - uint8_t ch2_3; - }; - - std::atomic sensor_{}; - -private: - class RoboCommand : public rmcs_executor::Component { - public: - explicit RoboCommand(ForceSensorSetting& robot) - : robot_(robot) {} - - void update() override { robot_.command_update(); } - - ForceSensorSetting& robot_; - }; - std::shared_ptr robot_command_; - - device::ForceSensorRuntime force_sensor_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorSetting, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp b/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp deleted file mode 100644 index 0527411d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/force_sensor_test.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "hardware/device/force_sensor_runtime.hpp" -#include "librmcs/client/cboard.hpp" -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware { - -class ForceSensorTest - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { - -public: - ForceSensorTest() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} - , robot_command_(create_partner_component(get_component_name() + "_command", *this)) - , force_sensor_(*this) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) {} - - ~ForceSensorTest() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { force_sensor_.update_status(); } - - void command_update() { - // uint16_t can_commands[4]; - - // can_commands[0] = 0; - // can_commands[1] = 0; - // can_commands[2] = 0; - // can_commands[3] = 0; - // transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); - - // can_commands[0] = 0; - // can_commands[1] = 0; - // can_commands[2] = 0; - // can_commands[3] = 0; - // transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - // can_commands[0] = 0; - // can_commands[1] = 0; - // can_commands[2] = 0; - // can_commands[3] = 0; - // transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - - // can_commands[0] = 0; - // can_commands[1] = 0; - // can_commands[2] = 0; - // can_commands[3] = 0; - // transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - - count_++; - if (count_ == 100) { - count_ = 0; - transmit_buffer_.add_can1_transmission(0x301, std::bit_cast(force_sensor_.generate_command())); - } - - transmit_buffer_.trigger_transmission(); - } - - int count_ = 0; - -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 == 0x302) { - force_sensor_.store_status(can_data); - sensor_.store(std::bit_cast(can_data), std::memory_order_relaxed); - auto data = sensor_.load(std::memory_order_relaxed); - - RCLCPP_INFO(get_logger(), "ch1: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch1_0, data.ch1_1, data.ch1_2, data.ch1_3); - RCLCPP_INFO(get_logger(), "ch2: 0x%2X 0x%2X 0x%2X 0x%2X", data.ch2_0, data.ch2_1, data.ch2_2, data.ch2_3); - } - } - - struct sensor_data { - uint8_t ch1_0; - uint8_t ch1_1; - uint8_t ch1_2; - uint8_t ch1_3; - uint8_t ch2_0; - uint8_t ch2_1; - uint8_t ch2_2; - uint8_t ch2_3; - }; - - std::atomic sensor_{}; - - // 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; - - // void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; - - // void uart2_receive_callback(const std::byte* data, uint8_t length) override; - - // void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override; - -private: - class RoboCommand : public rmcs_executor::Component { - public: - explicit RoboCommand(ForceSensorTest& robot) - : robot_(robot) {} - - void update() override { robot_.command_update(); } - - ForceSensorTest& robot_; - }; - std::shared_ptr robot_command_; - - device::ForceSensorRuntime force_sensor_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ForceSensorTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp index d0abbf47..bc2c3974 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/hero.cpp @@ -1,471 +1,480 @@ -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/benewake.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/gy614.hpp" -#include "hardware/device/hipnuc.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class Hero - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Hero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , command_component_( - create_partner_component(get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); - bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); - } - - ~Hero() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - class HeroCommand : public rmcs_executor::Component { - public: - explicit HeroCommand(Hero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - Hero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::client::CBoard { - public: - friend class Hero; - explicit TopBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , tf_(hero.tf_) - , imu_(1000, 0.2, 0.0) - , gy614_(hero, "/friction_wheels/temperature") - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) - // TODO: Bad CAN ID sequence, needs to be adjusted. - , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::M2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10}) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - - external_imu_thread_ = std::jthread([this, &hero](const std::stop_token& stop_token) { - external_imu_thread_main( - stop_token, hero.get_parameter("external_imu_port").as_string(), - hero.get_logger()); - }); - } - - ~TopBoard() final { - stop_handling_events(); - event_thread_.join(); - external_imu_thread_.request_stop(); - } - - void update() { - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - if (external_imu_available_.load(std::memory_order::relaxed)) { - external_imu_.update_status(); - gimbal_imu_pose.slerp(0.001, external_imu_.quaternion()); - imu_.q0() = gimbal_imu_pose.w(); - imu_.q1() = gimbal_imu_pose.x(); - imu_.q2() = gimbal_imu_pose.y(); - imu_.q3() = gimbal_imu_pose.z(); - } - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - gy614_.update_status(); - benewake_.update_status(); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_player_viewer_motor_.update_status(); - } - - void command_update() { - uint16_t batch_commands[4]; - for (int i = 0; i < 4; i++) - batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); - - batch_commands[0] = gimbal_scope_motor_.generate_command(); - batch_commands[1] = 0; - batch_commands[2] = 0; - batch_commands[3] = 0; - transmit_buffer_.add_can1_transmission(0x1ff, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission( - 0x141, gimbal_player_viewer_motor_.generate_angle_command( - gimbal_player_viewer_motor_.control_angle())); - - transmit_buffer_.trigger_transmission(); - } - - private: - 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 == 0x201) { - gimbal_friction_wheels_[0].store_status(can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x141) { - gimbal_player_viewer_motor_.store_status(can_data); - } - } - - void uart1_receive_callback(const std::byte* data, uint8_t length) override { - benewake_.store_status(data, length); - } - - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - gy614_.store_status(data, length); - } - - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - - void external_imu_thread_main( - const std::stop_token& stop_token, const std::string& port_name, - const rclcpp::Logger& logger) { - try { - serial::Serial serial{port_name, 115200, serial::Timeout::simpleTimeout(10)}; - rmcs_utility::FpsCounter fps_counter; - - while (!stop_token.stop_requested()) { - if (external_imu_.store_status(serial) && fps_counter.count()) { - bool available = fps_counter.fps() > 350.0; - if (!available) - RCLCPP_WARN(logger, "External IMU low FPS: %.2f", fps_counter.fps()); - else if (!external_imu_available_.load(std::memory_order::relaxed)) - RCLCPP_INFO( - logger, "External IMU now available with FPS: %.2f", - fps_counter.fps()); - external_imu_available_.store(available, std::memory_order::relaxed); - } - } - } catch (const std::exception& e) { - external_imu_available_.store(false, std::memory_order::relaxed); - RCLCPP_ERROR(logger, "Exception in external IMU thread: %s", e.what()); - } - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::Gy614 gy614_; - device::Benewake benewake_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::LkMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_friction_wheels_[4]; - - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - - rmcs_core::hardware::device::Hipnuc external_imu_; - std::atomic external_imu_available_ = false; - std::jthread external_imu_thread_; - }; - - class BottomBoard final : private librmcs::client::CBoard { - public: - friend class Hero; - explicit BottomBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}}) - , supercap_(hero, hero_command) - , gimbal_yaw_motor_( - hero, hero_command, "/gimbal/yaw", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("yaw_motor_zero_point").as_int()))) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_reversed() - .enable_multi_turn_angle()) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - hero.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; - }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } - - void update() { - imu_.update_status(); - dr16_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - - supercap_.update_status(); - - gimbal_yaw_motor_.update_status(); - tf_->set_state( - gimbal_yaw_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - uint16_t batch_commands[4]; - - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can1_transmission( - 0x141, gimbal_bullet_feeder_.generate_torque_command( - gimbal_bullet_feeder_.control_torque())); - - transmit_buffer_.add_can2_transmission(0x141, gimbal_yaw_motor_.generate_command()); - - batch_commands[0] = 0; - batch_commands[1] = 0; - batch_commands[2] = 0; - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(batch_commands)); - - transmit_buffer_.trigger_transmission(); - } - - private: - 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 == 0x201) { - chassis_wheel_motors_[0].store_status(can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(can_data); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x141) { - gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x300) { - supercap_.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); - } +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "hardware/device/benewake.hpp" +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/gy614.hpp" +// #include "hardware/device/hipnuc.hpp" +// #include "hardware/device/lk_motor.hpp" +// #include "hardware/device/supercap.hpp" + +// namespace rmcs_core::hardware { + +// class Hero +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// Hero() +// : Node{get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , +// command_component_( +// create_partner_component(get_component_name() + "_command", *this)) { +// using namespace rmcs_description; + +// register_output("/tf", tf_); +// tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + +// gimbal_calibrate_subscription_ = create_subscription( +// "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// gimbal_calibrate_subscription_callback(std::move(msg)); +// }); + +// top_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_top_board").as_int())); +// bottom_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_bottom_board").as_int())); +// } + +// ~Hero() override = default; + +// void update() override { +// top_board_->update(); +// bottom_board_->update(); +// } + +// void command_update() { +// top_board_->command_update(); +// bottom_board_->command_update(); +// } + +// private: +// void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New yaw offset: %ld", +// bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New pitch offset: %ld", +// top_board_->gimbal_pitch_motor_.calibrate_zero_point()); +// } + +// class HeroCommand : public rmcs_executor::Component { +// public: +// explicit HeroCommand(Hero& hero) +// : hero_(hero) {} + +// void update() override { hero_.command_update(); } + +// Hero& hero_; +// }; +// std::shared_ptr command_component_; + +// class TopBoard final : private librmcs::client::CBoard { +// public: +// friend class Hero; +// explicit TopBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) +// : librmcs::client::CBoard(usb_pid) +// , tf_(hero.tf_) +// , imu_(1000, 0.2, 0.0) +// , gy614_(hero, "/friction_wheels/temperature") +// , benewake_(hero, "/gimbal/auto_aim/laser_distance") +// , gimbal_pitch_motor_( +// hero, hero_command, "/gimbal/pitch", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) +// // TODO: Bad CAN ID sequence, needs to be adjusted. +// , gimbal_friction_wheels_( +// {hero, hero_command, "/gimbal/second_left_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, +// {hero, hero_command, "/gimbal/second_right_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(1.) +// .set_reversed()}, +// {hero, hero_command, "/gimbal/first_left_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, +// {hero, hero_command, "/gimbal/first_right_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(1.) +// .set_reversed()}) +// , gimbal_scope_motor_( +// hero, hero_command, "/gimbal/scope", +// device::DjiMotor::Config{device::DjiMotor::Type::M2006}) +// , gimbal_player_viewer_motor_( +// hero, hero_command, "/gimbal/player_viewer", +// device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10}) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// imu_.set_coordinate_mapping([](double x, double y, double z) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise use a +// // matrix. + +// // Eigen::AngleAxisd pitch_link_to_imu_link{ +// // std::numbers::pi, Eigen::Vector3d::UnitZ()}; +// // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; +// // std::cout << mapping << std::endl; + +// return std::make_tuple(x, y, z); +// }); + +// hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); +// hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + +// external_imu_thread_ = std::jthread([this, &hero](const std::stop_token& stop_token) +// { +// external_imu_thread_main( +// stop_token, hero.get_parameter("external_imu_port").as_string(), +// hero.get_logger()); +// }); +// } + +// ~TopBoard() final { +// stop_handling_events(); +// event_thread_.join(); +// external_imu_thread_.request_stop(); +// } + +// void update() { +// imu_.update_status(); +// Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + +// if (external_imu_available_.load(std::memory_order::relaxed)) { +// external_imu_.update_status(); +// gimbal_imu_pose.slerp(0.001, external_imu_.quaternion()); +// imu_.q0() = gimbal_imu_pose.w(); +// imu_.q1() = gimbal_imu_pose.x(); +// imu_.q2() = gimbal_imu_pose.y(); +// imu_.q3() = gimbal_imu_pose.z(); +// } + +// tf_->set_transform( +// gimbal_imu_pose.conjugate()); + +// gy614_.update_status(); +// benewake_.update_status(); + +// *gimbal_yaw_velocity_imu_ = imu_.gz(); +// *gimbal_pitch_velocity_imu_ = imu_.gy(); + +// gimbal_pitch_motor_.update_status(); +// tf_->set_state( +// gimbal_pitch_motor_.angle()); + +// for (auto& motor : gimbal_friction_wheels_) +// motor.update_status(); + +// gimbal_player_viewer_motor_.update_status(); +// } + +// void command_update() { +// uint16_t batch_commands[4]; +// for (int i = 0; i < 4; i++) +// batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can2_transmission(0x142, +// gimbal_pitch_motor_.generate_command()); + +// batch_commands[0] = gimbal_scope_motor_.generate_command(); +// batch_commands[1] = 0; +// batch_commands[2] = 0; +// batch_commands[3] = 0; +// transmit_buffer_.add_can1_transmission(0x1ff, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can2_transmission( +// 0x141, gimbal_player_viewer_motor_.generate_angle_command( +// gimbal_player_viewer_motor_.control_angle())); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// 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 == 0x201) { +// gimbal_friction_wheels_[0].store_status(can_data); +// } else if (can_id == 0x202) { +// gimbal_friction_wheels_[1].store_status(can_data); +// } else if (can_id == 0x203) { +// gimbal_friction_wheels_[2].store_status(can_data); +// } else if (can_id == 0x204) { +// gimbal_friction_wheels_[3].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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x142) { +// gimbal_pitch_motor_.store_status(can_data); +// } else if (can_id == 0x141) { +// gimbal_player_viewer_motor_.store_status(can_data); +// } +// } + +// void uart1_receive_callback(const std::byte* data, uint8_t length) override { +// benewake_.store_status(data, length); +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// gy614_.store_status(data, length); +// } + +// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// void external_imu_thread_main( +// const std::stop_token& stop_token, const std::string& port_name, +// const rclcpp::Logger& logger) { +// try { +// serial::Serial serial{port_name, 115200, serial::Timeout::simpleTimeout(10)}; +// rmcs_utility::FpsCounter fps_counter; + +// while (!stop_token.stop_requested()) { +// if (external_imu_.store_status(serial) && fps_counter.count()) { +// bool available = fps_counter.fps() > 350.0; +// if (!available) +// RCLCPP_WARN(logger, "External IMU low FPS: %.2f", fps_counter.fps()); +// else if (!external_imu_available_.load(std::memory_order::relaxed)) +// RCLCPP_INFO( +// logger, "External IMU now available with FPS: %.2f", +// fps_counter.fps()); +// external_imu_available_.store(available, std::memory_order::relaxed); +// } +// } +// } catch (const std::exception& e) { +// external_imu_available_.store(false, std::memory_order::relaxed); +// RCLCPP_ERROR(logger, "Exception in external IMU thread: %s", e.what()); +// } +// } + +// OutputInterface& tf_; + +// device::Bmi088 imu_; +// device::Gy614 gy614_; +// device::Benewake benewake_; + +// OutputInterface gimbal_yaw_velocity_imu_; +// OutputInterface gimbal_pitch_velocity_imu_; + +// device::LkMotor gimbal_pitch_motor_; + +// device::DjiMotor gimbal_friction_wheels_[4]; + +// device::DjiMotor gimbal_scope_motor_; +// device::LkMotor gimbal_player_viewer_motor_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; + +// rmcs_core::hardware::device::Hipnuc external_imu_; +// std::atomic external_imu_available_ = false; +// std::jthread external_imu_thread_; +// }; + +// class BottomBoard final : private librmcs::client::CBoard { +// public: +// friend class Hero; +// explicit BottomBoard(Hero& hero, HeroCommand& hero_command, int usb_pid = -1) +// : librmcs::client::CBoard(usb_pid) +// , imu_(1000, 0.2, 0.0) +// , tf_(hero.tf_) +// , dr16_(hero) +// , chassis_wheel_motors_( +// {hero, hero_command, "/chassis/left_front_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, +// {hero, hero_command, "/chassis/left_back_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, +// {hero, hero_command, "/chassis/right_back_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}}, +// {hero, hero_command, "/chassis/right_front_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}}) +// , supercap_(hero, hero_command) +// , gimbal_yaw_motor_( +// hero, hero_command, "/gimbal/yaw", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("yaw_motor_zero_point").as_int()))) +// , gimbal_bullet_feeder_( +// hero, hero_command, "/gimbal/bullet_feeder", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_reversed() +// .enable_multi_turn_angle()) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// hero.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; +// }; + +// hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); +// } + +// ~BottomBoard() final { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() { +// imu_.update_status(); +// dr16_.update_status(); + +// *chassis_yaw_velocity_imu_ = imu_.gz(); + +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); + +// supercap_.update_status(); + +// gimbal_yaw_motor_.update_status(); +// tf_->set_state( +// gimbal_yaw_motor_.angle()); + +// gimbal_bullet_feeder_.update_status(); +// } + +// void command_update() { +// uint16_t batch_commands[4]; + +// for (int i = 0; i < 4; i++) +// batch_commands[i] = chassis_wheel_motors_[i].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can1_transmission( +// 0x141, gimbal_bullet_feeder_.generate_torque_command( +// gimbal_bullet_feeder_.control_torque())); + +// transmit_buffer_.add_can2_transmission(0x141, gimbal_yaw_motor_.generate_command()); + +// batch_commands[0] = 0; +// batch_commands[1] = 0; +// batch_commands[2] = 0; +// batch_commands[3] = supercap_.generate_command(); +// transmit_buffer_.add_can2_transmission(0x1FE, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// 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 == 0x201) { +// chassis_wheel_motors_[0].store_status(can_data); +// } else if (can_id == 0x202) { +// chassis_wheel_motors_[1].store_status(can_data); +// } else if (can_id == 0x203) { +// chassis_wheel_motors_[2].store_status(can_data); +// } else if (can_id == 0x204) { +// chassis_wheel_motors_[3].store_status(can_data); +// } else if (can_id == 0x141) { +// gimbal_bullet_feeder_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x141) { +// gimbal_yaw_motor_.store_status(can_data); +// } else if (can_id == 0x300) { +// supercap_.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 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 { - imu_.store_accelerometer_status(x, y, z); - } +// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } - device::Bmi088 imu_; - OutputInterface& tf_; +// device::Bmi088 imu_; +// OutputInterface& tf_; - OutputInterface chassis_yaw_velocity_imu_; +// OutputInterface chassis_yaw_velocity_imu_; - device::Dr16 dr16_; +// device::Dr16 dr16_; - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; +// device::DjiMotor chassis_wheel_motors_[4]; +// device::Supercap supercap_; - device::LkMotor gimbal_yaw_motor_; +// device::LkMotor gimbal_yaw_motor_; - device::LkMotor gimbal_bullet_feeder_; +// device::LkMotor gimbal_bullet_feeder_; - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; +// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; +// OutputInterface referee_serial_; - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - }; +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// }; - OutputInterface tf_; +// OutputInterface tf_; - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; -}; +// std::unique_ptr top_board_; +// std::unique_ptr bottom_board_; +// }; -} // namespace rmcs_core::hardware +// } // namespace rmcs_core::hardware -#include +// #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Hero, rmcs_executor::Component) \ No newline at end of file +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Hero, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp index f4592d7f..e4ccabf5 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/infantry.cpp @@ -1,312 +1,315 @@ -#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/gy614.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class Infantry - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { -public: - Infantry() - : 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()) - , infantry_command_( - create_partner_component(get_component_name() + "_command", *this)) - , chassis_wheel_motors_( - {*this, *infantry_command_, "/chassis/left_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_back_wheel"}, - {*this, *infantry_command_, "/chassis/left_back_wheel"}) - , supercap_(*this, *infantry_command_) - , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") - , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") - , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") - , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") - , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} - , bmi088_(1000, 0.2, 0.0) - , gy614_(*this, "/friction_wheels/temperature") - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(13.) - .enable_multi_turn_angle()); - - gimbal_yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.set_encoder_zero_point( - static_cast(get_parameter("yaw_motor_zero_point").as_int()))); - gimbal_pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.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_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .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) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-y, x, z); - }); - - using namespace rmcs_description; - tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); - - constexpr double gimbal_center_height = 0.32059; - constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; - tf_->set_transform( - Eigen::Translation3d{0, 0, gimbal_center_height}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - - 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; - }; - } - - ~Infantry() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { - update_motors(); - update_imu(); - dr16_.update_status(); - gy614_.update_status(); - supercap_.update_status(); - } - - void command_update() { - uint16_t can_commands[4]; - - can_commands[0] = gimbal_yaw_motor_.generate_command(); - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = chassis_wheel_motors_[0].generate_command(); - can_commands[1] = chassis_wheel_motors_[1].generate_command(); - can_commands[2] = chassis_wheel_motors_[2].generate_command(); - can_commands[3] = chassis_wheel_motors_[3].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = 0; - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = 0; - can_commands[1] = gimbal_bullet_feeder_.generate_command(); - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.trigger_transmission(); - } - -private: - void update_motors() { - using namespace rmcs_description; - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - tf_->set_state(chassis_wheel_motors_[0].angle()); - tf_->set_state(chassis_wheel_motors_[1].angle()); - tf_->set_state(chassis_wheel_motors_[2].angle()); - tf_->set_state(chassis_wheel_motors_[3].angle()); - - 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: %d", - gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - logger_, "[gimbal calibration] New pitch offset: %d", - 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 == 0x201) { - auto& motor = chassis_wheel_motors_[0]; - motor.store_status(can_data); - } else if (can_id == 0x202) { - auto& motor = chassis_wheel_motors_[1]; - motor.store_status(can_data); - } else if (can_id == 0x203) { - auto& motor = chassis_wheel_motors_[2]; - motor.store_status(can_data); - } else if (can_id == 0x204) { - auto& motor = chassis_wheel_motors_[3]; - motor.store_status(can_data); - } else if (can_id == 0x205) { - gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x206) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x300) { - supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x202) { - gimbal_bullet_feeder_.store_status(can_data); - } else if (can_id == 0x203) { - gimbal_left_friction_.store_status(can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.store_status(can_data); - } else if (can_id == 0x206) { - 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 uart2_receive_callback(const std::byte* data, uint8_t length) override { - gy614_.store_status(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 InfantryCommand : public rmcs_executor::Component { - public: - explicit InfantryCommand(Infantry& infantry) - : infantry_(infantry) {} - - void update() override { infantry_.command_update(); } - - Infantry& infantry_; - }; - std::shared_ptr infantry_command_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::DjiMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - device::DjiMotor gimbal_bullet_feeder_; - - device::Dr16 dr16_; - device::Bmi088 bmi088_; - device::Gy614 gy614_; - - 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::Infantry, rmcs_executor::Component) \ No newline at end of file +// #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/gy614.hpp" +// #include "hardware/device/supercap.hpp" + +// namespace rmcs_core::hardware { + +// class Infantry +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// Infantry() +// : 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()) , infantry_command_( +// create_partner_component(get_component_name() + "_command", +// *this)) +// , chassis_wheel_motors_( +// {*this, *infantry_command_, "/chassis/left_front_wheel"}, +// {*this, *infantry_command_, "/chassis/right_front_wheel"}, +// {*this, *infantry_command_, "/chassis/right_back_wheel"}, +// {*this, *infantry_command_, "/chassis/left_back_wheel"}) +// , supercap_(*this, *infantry_command_) +// , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") +// , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") +// , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") +// , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") +// , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") +// , dr16_{*this} +// , bmi088_(1000, 0.2, 0.0) +// , gy614_(*this, "/friction_wheels/temperature") +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// for (auto& motor : chassis_wheel_motors_) +// motor.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(13.) +// .enable_multi_turn_angle()); + +// gimbal_yaw_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.set_encoder_zero_point( +// static_cast(get_parameter("yaw_motor_zero_point").as_int()))); +// gimbal_pitch_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020}.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_reduction_ratio(1.)); +// gimbal_right_friction_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .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) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise use a +// matrix. + +// // Eigen::AngleAxisd pitch_link_to_imu_link{ +// // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; +// // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; +// // std::cout << mapping << std::endl; + +// return std::make_tuple(-y, x, z); +// }); + +// using namespace rmcs_description; +// tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); + +// constexpr double gimbal_center_height = 0.32059; +// constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; +// tf_->set_transform( +// Eigen::Translation3d{0, 0, gimbal_center_height}); +// tf_->set_transform( +// Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); + +// 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; +// }; +// } + +// ~Infantry() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// update_motors(); +// update_imu(); +// dr16_.update_status(); +// gy614_.update_status(); +// supercap_.update_status(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// can_commands[0] = gimbal_yaw_motor_.generate_command(); +// can_commands[1] = gimbal_pitch_motor_.generate_command(); +// can_commands[2] = 0; +// can_commands[3] = supercap_.generate_command(); +// transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); + +// can_commands[0] = chassis_wheel_motors_[0].generate_command(); +// can_commands[1] = chassis_wheel_motors_[1].generate_command(); +// can_commands[2] = chassis_wheel_motors_[2].generate_command(); +// can_commands[3] = chassis_wheel_motors_[3].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + +// can_commands[0] = 0; +// can_commands[1] = gimbal_pitch_motor_.generate_command(); +// can_commands[2] = 0; +// can_commands[3] = 0; +// transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(can_commands)); + +// can_commands[0] = 0; +// can_commands[1] = gimbal_bullet_feeder_.generate_command(); +// can_commands[2] = gimbal_left_friction_.generate_command(); +// can_commands[3] = gimbal_right_friction_.generate_command(); +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// void update_motors() { +// using namespace rmcs_description; +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); +// tf_->set_state(chassis_wheel_motors_[0].angle()); +// tf_->set_state(chassis_wheel_motors_[1].angle()); +// tf_->set_state(chassis_wheel_motors_[2].angle()); +// tf_->set_state(chassis_wheel_motors_[3].angle()); + +// 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: %d", +// gimbal_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// logger_, "[gimbal calibration] New pitch offset: %d", +// 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 == 0x201) { +// auto& motor = chassis_wheel_motors_[0]; +// motor.store_status(can_data); +// } else if (can_id == 0x202) { +// auto& motor = chassis_wheel_motors_[1]; +// motor.store_status(can_data); +// } else if (can_id == 0x203) { +// auto& motor = chassis_wheel_motors_[2]; +// motor.store_status(can_data); +// } else if (can_id == 0x204) { +// auto& motor = chassis_wheel_motors_[3]; +// motor.store_status(can_data); +// } else if (can_id == 0x205) { +// gimbal_yaw_motor_.store_status(can_data); +// } else if (can_id == 0x206) { +// gimbal_pitch_motor_.store_status(can_data); +// } else if (can_id == 0x300) { +// supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x202) { +// gimbal_bullet_feeder_.store_status(can_data); +// } else if (can_id == 0x203) { +// gimbal_left_friction_.store_status(can_data); +// } else if (can_id == 0x204) { +// gimbal_right_friction_.store_status(can_data); +// } else if (can_id == 0x206) { +// 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 uart2_receive_callback(const std::byte* data, uint8_t length) override { +// gy614_.store_status(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 InfantryCommand : public rmcs_executor::Component { +// public: +// explicit InfantryCommand(Infantry& infantry) +// : infantry_(infantry) {} + +// void update() override { infantry_.command_update(); } + +// Infantry& infantry_; +// }; +// std::shared_ptr infantry_command_; + +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + +// device::DjiMotor chassis_wheel_motors_[4]; +// device::Supercap supercap_; + +// device::DjiMotor gimbal_yaw_motor_; +// device::DjiMotor gimbal_pitch_motor_; + +// device::DjiMotor gimbal_left_friction_; +// device::DjiMotor gimbal_right_friction_; +// device::DjiMotor gimbal_bullet_feeder_; + +// device::Dr16 dr16_; +// device::Bmi088 bmi088_; +// device::Gy614 gy614_; + +// 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::Infantry, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp index 16f1bcca..17b9154c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-hero.cpp @@ -1,517 +1,528 @@ -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/benewake.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class SteeringHero - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringHero() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - - register_output("/tf", tf_); - tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); - bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); - - temperature_logging_timer_.reset(1000); - } - - ~SteeringHero() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - - if (temperature_logging_timer_.tick()) { - temperature_logging_timer_.reset(1000); - RCLCPP_INFO( - get_logger(), - "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", - top_board_->gimbal_pitch_motor_.temperature(), - top_board_->gimbal_top_yaw_motor_.temperature(), - bottom_board_->gimbal_bottom_yaw_motor_.temperature(), - top_board_->gimbal_bullet_feeder_.temperature()); - } - } - - void command_update() { - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", - bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New top yaw offset: %ld", - top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New viewer offset: %ld", - top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left front steering offset: %d", - bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] left back steering offset: %d", - bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right back steering offset: %d", - bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[chassis calibration] right front steering offset: %d", - bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); - } - - class SteeringHeroCommand : public rmcs_executor::Component { - public: - explicit SteeringHeroCommand(SteeringHero& hero) - : hero_(hero) {} - - void update() override { hero_.command_update(); } - - SteeringHero& hero_; - }; - std::shared_ptr command_component_; - - class TopBoard final : private librmcs::client::CBoard { - public: - friend class SteeringHero; - explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , tf_(hero.tf_) - , imu_(1000, 0.2, 0.0) - , benewake_(hero, "/gimbal/auto_aim/laser_distance") - , gimbal_top_yaw_motor_( - hero, hero_command, "/gimbal/top_yaw", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast( - hero.get_parameter("top_yaw_motor_zero_point").as_int()))) - , gimbal_pitch_motor_( - hero, hero_command, "/gimbal/pitch", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) - , gimbal_friction_wheels_( - {hero, hero_command, "/gimbal/second_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, - {hero, hero_command, "/gimbal/first_left_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, - {hero, hero_command, "/gimbal/first_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}, - {hero, hero_command, "/gimbal/second_right_friction", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()}) - , gimbal_bullet_feeder_( - hero, hero_command, "/gimbal/bullet_feeder", - device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} - .set_reversed() - .enable_multi_turn_angle()) - , gimbal_scope_motor_( - hero, hero_command, "/gimbal/scope", - device::DjiMotor::Config{device::DjiMotor::Type::M2006}) - , gimbal_player_viewer_motor_( - hero, hero_command, "/gimbal/player_viewer", - device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10} - .set_encoder_zero_point( - static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) - .set_reversed()) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - imu_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a - // matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-x, -y, z); - }); - - hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); - hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); - } - - ~TopBoard() final { - stop_handling_events(); - event_thread_.join(); - } - - void update() { - imu_.update_status(); - Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; - - tf_->set_transform( - gimbal_imu_pose.conjugate()); - - benewake_.update_status(); - - *gimbal_yaw_velocity_imu_ = imu_.gz(); - *gimbal_pitch_velocity_imu_ = imu_.gy(); - - gimbal_top_yaw_motor_.update_status(); - - gimbal_pitch_motor_.update_status(); - tf_->set_state( - gimbal_pitch_motor_.angle()); - - gimbal_player_viewer_motor_.update_status(); - tf_->set_state( - gimbal_player_viewer_motor_.angle()); - - gimbal_scope_motor_.update_status(); - - for (auto& motor : gimbal_friction_wheels_) - motor.update_status(); - - gimbal_bullet_feeder_.update_status(); - } - - void command_update() { - uint16_t batch_commands[4]{}; - - for (int i = 0; i < 4; i++) - batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can1_transmission( - 0x141, gimbal_bullet_feeder_.generate_torque_command( - gimbal_bullet_feeder_.control_torque())); - - batch_commands[0] = gimbal_scope_motor_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission( - 0x143, gimbal_player_viewer_motor_.generate_velocity_command( - gimbal_player_viewer_motor_.control_velocity())); - - transmit_buffer_.add_can2_transmission(0x141, gimbal_top_yaw_motor_.generate_command()); - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); - - transmit_buffer_.trigger_transmission(); - } - - private: - 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 == 0x201) { - gimbal_friction_wheels_[0].store_status(can_data); - } else if (can_id == 0x202) { - gimbal_friction_wheels_[1].store_status(can_data); - } else if (can_id == 0x203) { - gimbal_friction_wheels_[2].store_status(can_data); - } else if (can_id == 0x204) { - gimbal_friction_wheels_[3].store_status(can_data); - } else if (can_id == 0x141) { - gimbal_bullet_feeder_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x141) { - gimbal_top_yaw_motor_.store_status(can_data); - } else if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x143) { - gimbal_player_viewer_motor_.store_status(can_data); - } else if (can_id == 0x201) { - gimbal_scope_motor_.store_status(can_data); - } - } - - void uart2_receive_callback(const std::byte* data, uint8_t length) override { - benewake_.store_status(data, length); - } - - void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - - OutputInterface& tf_; - - device::Bmi088 imu_; - device::Benewake benewake_; - - OutputInterface gimbal_yaw_velocity_imu_; - OutputInterface gimbal_pitch_velocity_imu_; - - device::LkMotor gimbal_top_yaw_motor_; - device::LkMotor gimbal_pitch_motor_; - - device::DjiMotor gimbal_friction_wheels_[4]; - device::LkMotor gimbal_bullet_feeder_; - - device::DjiMotor gimbal_scope_motor_; - device::LkMotor gimbal_player_viewer_motor_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - }; - - class BottomBoard final : private librmcs::client::CBoard { - public: - friend class SteeringHero; - explicit BottomBoard( - SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , imu_(1000, 0.2, 0.0) - , tf_(hero.tf_) - , dr16_(hero) - , chassis_steering_motors_( - {hero, hero_command, "/chassis/left_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_front_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/left_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("left_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_back_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_back_zero_point").as_int())) - .set_reversed()}, - {hero, hero_command, "/chassis/right_front_steering", - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_encoder_zero_point( - static_cast(hero.get_parameter("right_front_zero_point").as_int())) - .set_reversed()}) - , chassis_wheel_motors_( - {hero, hero_command, "/chassis/left_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/left_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_back_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}, - {hero, hero_command, "/chassis/right_front_wheel", - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(2232. / 169.)}) - , supercap_(hero, hero_command) - , gimbal_bottom_yaw_motor_( - hero, hero_command, "/gimbal/bottom_yaw", - device::LkMotor::Config{device::LkMotor::Type::MG6012E_I8} - .set_reversed() - .set_encoder_zero_point( - static_cast( - hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - hero.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; - }; - - hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - - hero.register_output( - "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); - hero.register_output( - "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); - } - - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } - - void update() { - imu_.update_status(); - dr16_.update_status(); - supercap_.update_status(); - - *chassis_yaw_velocity_imu_ = imu_.gz(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steering_motors_) - motor.update_status(); - gimbal_bottom_yaw_motor_.update_status(); - tf_->set_state( - gimbal_bottom_yaw_motor_.angle()); - } - - void command_update() { - uint16_t batch_commands[4]{}; - - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(batch_commands)); - - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_steering_motors_[i].generate_command(); - transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(batch_commands)); - - transmit_buffer_.add_can2_transmission( - 0x141, gimbal_bottom_yaw_motor_.generate_command()); - - transmit_buffer_.trigger_transmission(); - } - - private: - 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 == 0x201) { - chassis_wheel_motors_[0].store_status(can_data); - } else if (can_id == 0x202) { - chassis_wheel_motors_[1].store_status(can_data); - } else if (can_id == 0x203) { - chassis_wheel_motors_[2].store_status(can_data); - } else if (can_id == 0x204) { - chassis_wheel_motors_[3].store_status(can_data); - } else if (can_id == 0x300) { - supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x205) { - chassis_steering_motors_[0].store_status(can_data); - } else if (can_id == 0x206) { - chassis_steering_motors_[1].store_status(can_data); - } else if (can_id == 0x207) { - chassis_steering_motors_[2].store_status(can_data); - } else if (can_id == 0x208) { - chassis_steering_motors_[3].store_status(can_data); - } else if (can_id == 0x141) { - gimbal_bottom_yaw_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 { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - - device::Bmi088 imu_; - OutputInterface& tf_; - - OutputInterface chassis_yaw_velocity_imu_; - - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - - device::DjiMotor chassis_steering_motors_[4]; - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::LkMotor gimbal_bottom_yaw_motor_; - - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - }; - - OutputInterface tf_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - std::unique_ptr top_board_; - std::unique_ptr bottom_board_; - - rmcs_utility::TickTimer temperature_logging_timer_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) \ No newline at end of file +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "hardware/device/benewake.hpp" +// #include "hardware/device/bmi088.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/dr16.hpp" +// #include "hardware/device/lk_motor.hpp" +// #include "hardware/device/supercap.hpp" + +// namespace rmcs_core::hardware { + +// class SteeringHero +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// SteeringHero() +// : Node{get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , +// command_component_( +// create_partner_component( +// get_component_name() + "_command", *this)) { +// using namespace rmcs_description; + +// register_output("/tf", tf_); +// tf_->set_transform(Eigen::Translation3d{0.16, 0.0, 0.15}); + +// gimbal_calibrate_subscription_ = create_subscription( +// "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// gimbal_calibrate_subscription_callback(std::move(msg)); +// }); + +// top_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_top_board").as_int())); +// bottom_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_bottom_board").as_int())); + +// temperature_logging_timer_.reset(1000); +// } + +// ~SteeringHero() override = default; + +// void update() override { +// top_board_->update(); +// bottom_board_->update(); + +// if (temperature_logging_timer_.tick()) { +// temperature_logging_timer_.reset(1000); +// RCLCPP_INFO( +// get_logger(), +// "Temperature: pitch: %.1f, top_yaw: %.1f, bottom_yaw: %.1f, feeder: %.1f", +// top_board_->gimbal_pitch_motor_.temperature(), +// top_board_->gimbal_top_yaw_motor_.temperature(), +// bottom_board_->gimbal_bottom_yaw_motor_.temperature(), +// top_board_->gimbal_bullet_feeder_.temperature()); +// } +// } + +// void command_update() { +// top_board_->command_update(); +// bottom_board_->command_update(); +// } + +// private: +// void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New bottom yaw offset: %ld", +// bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New pitch offset: %ld", +// top_board_->gimbal_pitch_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New top yaw offset: %ld", +// top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New viewer offset: %ld", +// top_board_->gimbal_player_viewer_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] left front steering offset: %d", +// bottom_board_->chassis_steering_motors_[0].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] left back steering offset: %d", +// bottom_board_->chassis_steering_motors_[1].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] right back steering offset: %d", +// bottom_board_->chassis_steering_motors_[2].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[chassis calibration] right front steering offset: %d", +// bottom_board_->chassis_steering_motors_[3].calibrate_zero_point()); +// } + +// class SteeringHeroCommand : public rmcs_executor::Component { +// public: +// explicit SteeringHeroCommand(SteeringHero& hero) +// : hero_(hero) {} + +// void update() override { hero_.command_update(); } + +// SteeringHero& hero_; +// }; +// std::shared_ptr command_component_; + +// class TopBoard final : private librmcs::client::CBoard { +// public: +// friend class SteeringHero; +// explicit TopBoard(SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = +// -1) +// : librmcs::client::CBoard(usb_pid) +// , tf_(hero.tf_) +// , imu_(1000, 0.2, 0.0) +// , benewake_(hero, "/gimbal/auto_aim/laser_distance") +// , gimbal_top_yaw_motor_( +// hero, hero_command, "/gimbal/top_yaw", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_encoder_zero_point( +// static_cast( +// hero.get_parameter("top_yaw_motor_zero_point").as_int()))) +// , gimbal_pitch_motor_( +// hero, hero_command, "/gimbal/pitch", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("pitch_motor_zero_point").as_int()))) +// , gimbal_friction_wheels_( +// {hero, hero_command, "/gimbal/second_left_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, +// {hero, hero_command, "/gimbal/first_left_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)}, +// {hero, hero_command, "/gimbal/first_right_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(1.) +// .set_reversed()}, +// {hero, hero_command, "/gimbal/second_right_friction", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(1.) +// .set_reversed()}) +// , gimbal_bullet_feeder_( +// hero, hero_command, "/gimbal/bullet_feeder", +// device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} +// .set_reversed() +// .enable_multi_turn_angle()) +// , gimbal_scope_motor_( +// hero, hero_command, "/gimbal/scope", +// device::DjiMotor::Config{device::DjiMotor::Type::M2006}) +// , gimbal_player_viewer_motor_( +// hero, hero_command, "/gimbal/player_viewer", +// device::LkMotor::Config{device::LkMotor::Type::MG4005E_I10} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("viewer_motor_zero_point").as_int())) +// .set_reversed()) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// imu_.set_coordinate_mapping([](double x, double y, double z) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise use a +// // matrix. + +// // Eigen::AngleAxisd pitch_link_to_imu_link{ +// // std::numbers::pi, Eigen::Vector3d::UnitZ()}; +// // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; +// // std::cout << mapping << std::endl; + +// return std::make_tuple(-x, -y, z); +// }); + +// hero.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); +// hero.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); +// } + +// ~TopBoard() final { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() { +// imu_.update_status(); +// Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + +// tf_->set_transform( +// gimbal_imu_pose.conjugate()); + +// benewake_.update_status(); + +// *gimbal_yaw_velocity_imu_ = imu_.gz(); +// *gimbal_pitch_velocity_imu_ = imu_.gy(); + +// gimbal_top_yaw_motor_.update_status(); + +// gimbal_pitch_motor_.update_status(); +// tf_->set_state( +// gimbal_pitch_motor_.angle()); + +// gimbal_player_viewer_motor_.update_status(); +// tf_->set_state( +// gimbal_player_viewer_motor_.angle()); + +// gimbal_scope_motor_.update_status(); + +// for (auto& motor : gimbal_friction_wheels_) +// motor.update_status(); + +// gimbal_bullet_feeder_.update_status(); +// } + +// void command_update() { +// uint16_t batch_commands[4]{}; + +// for (int i = 0; i < 4; i++) +// batch_commands[i] = gimbal_friction_wheels_[i].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can1_transmission( +// 0x141, gimbal_bullet_feeder_.generate_torque_command( +// gimbal_bullet_feeder_.control_torque())); + +// batch_commands[0] = gimbal_scope_motor_.generate_command(); +// transmit_buffer_.add_can2_transmission(0x200, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can2_transmission( +// 0x143, gimbal_player_viewer_motor_.generate_velocity_command( +// gimbal_player_viewer_motor_.control_velocity())); + +// transmit_buffer_.add_can2_transmission(0x141, +// gimbal_top_yaw_motor_.generate_command()); +// transmit_buffer_.add_can2_transmission(0x142, +// gimbal_pitch_motor_.generate_command()); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// 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 == 0x201) { +// gimbal_friction_wheels_[0].store_status(can_data); +// } else if (can_id == 0x202) { +// gimbal_friction_wheels_[1].store_status(can_data); +// } else if (can_id == 0x203) { +// gimbal_friction_wheels_[2].store_status(can_data); +// } else if (can_id == 0x204) { +// gimbal_friction_wheels_[3].store_status(can_data); +// } else if (can_id == 0x141) { +// gimbal_bullet_feeder_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x141) { +// gimbal_top_yaw_motor_.store_status(can_data); +// } else if (can_id == 0x142) { +// gimbal_pitch_motor_.store_status(can_data); +// } else if (can_id == 0x143) { +// gimbal_player_viewer_motor_.store_status(can_data); +// } else if (can_id == 0x201) { +// gimbal_scope_motor_.store_status(can_data); +// } +// } + +// void uart2_receive_callback(const std::byte* data, uint8_t length) override { +// benewake_.store_status(data, length); +// } + +// void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// OutputInterface& tf_; + +// device::Bmi088 imu_; +// device::Benewake benewake_; + +// OutputInterface gimbal_yaw_velocity_imu_; +// OutputInterface gimbal_pitch_velocity_imu_; + +// device::LkMotor gimbal_top_yaw_motor_; +// device::LkMotor gimbal_pitch_motor_; + +// device::DjiMotor gimbal_friction_wheels_[4]; +// device::LkMotor gimbal_bullet_feeder_; + +// device::DjiMotor gimbal_scope_motor_; +// device::LkMotor gimbal_player_viewer_motor_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// }; + +// class BottomBoard final : private librmcs::client::CBoard { +// public: +// friend class SteeringHero; +// explicit BottomBoard( +// SteeringHero& hero, SteeringHeroCommand& hero_command, int usb_pid = -1) +// : librmcs::client::CBoard(usb_pid) +// , imu_(1000, 0.2, 0.0) +// , tf_(hero.tf_) +// , dr16_(hero) +// , chassis_steering_motors_( +// {hero, hero_command, "/chassis/left_front_steering", +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("left_front_zero_point").as_int())) +// .set_reversed()}, +// {hero, hero_command, "/chassis/left_back_steering", +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("left_back_zero_point").as_int())) +// .set_reversed()}, +// {hero, hero_command, "/chassis/right_back_steering", +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("right_back_zero_point").as_int())) +// .set_reversed()}, +// {hero, hero_command, "/chassis/right_front_steering", +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_encoder_zero_point( +// static_cast(hero.get_parameter("right_front_zero_point").as_int())) +// .set_reversed()}) +// , chassis_wheel_motors_( +// {hero, hero_command, "/chassis/left_front_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)}, +// {hero, hero_command, "/chassis/left_back_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)}, +// {hero, hero_command, "/chassis/right_back_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)}, +// {hero, hero_command, "/chassis/right_front_wheel", +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(2232. / 169.)}) +// , supercap_(hero, hero_command) +// , gimbal_bottom_yaw_motor_( +// hero, hero_command, "/gimbal/bottom_yaw", +// device::LkMotor::Config{device::LkMotor::Type::MG6012E_I8} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// hero.get_parameter("bottom_yaw_motor_zero_point").as_int()))) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// hero.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; +// }; + +// hero.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + +// hero.register_output( +// "/chassis/powermeter/control_enable", powermeter_control_enabled_, false); +// hero.register_output( +// "/chassis/powermeter/charge_power_limit", powermeter_charge_power_limit_, 0.); +// } + +// ~BottomBoard() final { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() { +// imu_.update_status(); +// dr16_.update_status(); +// supercap_.update_status(); + +// *chassis_yaw_velocity_imu_ = imu_.gz(); + +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); +// for (auto& motor : chassis_steering_motors_) +// motor.update_status(); +// gimbal_bottom_yaw_motor_.update_status(); +// tf_->set_state( +// gimbal_bottom_yaw_motor_.angle()); +// } + +// void command_update() { +// uint16_t batch_commands[4]{}; + +// for (int i = 0; i < 4; i++) +// batch_commands[i] = chassis_wheel_motors_[i].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, +// std::bit_cast(batch_commands)); + +// batch_commands[3] = supercap_.generate_command(); +// transmit_buffer_.add_can1_transmission(0x1FE, +// std::bit_cast(batch_commands)); + +// for (int i = 0; i < 4; i++) +// batch_commands[i] = chassis_steering_motors_[i].generate_command(); +// transmit_buffer_.add_can2_transmission(0x1FE, +// std::bit_cast(batch_commands)); + +// transmit_buffer_.add_can2_transmission( +// 0x141, gimbal_bottom_yaw_motor_.generate_command()); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// 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 == 0x201) { +// chassis_wheel_motors_[0].store_status(can_data); +// } else if (can_id == 0x202) { +// chassis_wheel_motors_[1].store_status(can_data); +// } else if (can_id == 0x203) { +// chassis_wheel_motors_[2].store_status(can_data); +// } else if (can_id == 0x204) { +// chassis_wheel_motors_[3].store_status(can_data); +// } else if (can_id == 0x300) { +// supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x205) { +// chassis_steering_motors_[0].store_status(can_data); +// } else if (can_id == 0x206) { +// chassis_steering_motors_[1].store_status(can_data); +// } else if (can_id == 0x207) { +// chassis_steering_motors_[2].store_status(can_data); +// } else if (can_id == 0x208) { +// chassis_steering_motors_[3].store_status(can_data); +// } else if (can_id == 0x141) { +// gimbal_bottom_yaw_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 { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// device::Bmi088 imu_; +// OutputInterface& tf_; + +// OutputInterface chassis_yaw_velocity_imu_; + +// OutputInterface powermeter_control_enabled_; +// OutputInterface powermeter_charge_power_limit_; + +// device::Dr16 dr16_; + +// device::DjiMotor chassis_steering_motors_[4]; +// device::DjiMotor chassis_wheel_motors_[4]; +// device::Supercap supercap_; + +// device::LkMotor gimbal_bottom_yaw_motor_; + +// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; +// OutputInterface referee_serial_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// }; + +// OutputInterface tf_; + +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + +// std::unique_ptr top_board_; +// std::unique_ptr bottom_board_; + +// rmcs_utility::TickTimer temperature_logging_timer_; +// }; + +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringHero, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp index 91a1cb68..b6c7be93 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/steering-infantry.cpp @@ -1,444 +1,443 @@ -#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 "hardware/device/supercap.hpp" -#include "librmcs/client/cboard.hpp" - -namespace rmcs_core::hardware { - -class SteeringInfantry - : public rmcs_executor::Component - , public rclcpp::Node { -public: - SteeringInfantry() - : Node( - get_component_name(), - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) - , command_component_( - create_partner_component( - get_component_name() + "_command", *this)) { - using namespace rmcs_description; - register_output("/tf", tf_); - - gimbal_calibrate_subscription_ = create_subscription( - "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - gimbal_calibrate_subscription_callback(std::move(msg)); - }); - steers_calibrate_subscription_ = create_subscription( - "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { - steers_calibrate_subscription_callback(std::move(msg)); - }); - - top_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_top_board").as_int())); - - bottom_board_ = std::make_unique( - *this, *command_component_, - static_cast(get_parameter("usb_pid_bottom_board").as_int())); - - using namespace rmcs_description; - tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); - } - ~SteeringInfantry() override = default; - - void update() override { - top_board_->update(); - bottom_board_->update(); - } - - void command_update() { - - top_board_->command_update(); - bottom_board_->command_update(); - } - -private: - void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New yaw offset: %ld", - bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[gimbal calibration] New pitch offset: %ld", - top_board_->gimbal_pitch_motor_.calibrate_zero_point()); - } - - void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { - RCLCPP_INFO( - get_logger(), "[steer calibration] New left front offset: %d", - bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New left back offset: %d", - bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right back offset: %d", - bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); - RCLCPP_INFO( - get_logger(), "[steer calibration] New right front offset: %d", - bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); - } - class SteeringInfantryCommand : public rmcs_executor::Component { - public: - explicit SteeringInfantryCommand(SteeringInfantry& steeringInfantry) - : steeringInfantry(steeringInfantry) {} - - void update() override { steeringInfantry.command_update(); } - - SteeringInfantry& steeringInfantry; - }; - - class TopBoard final : private librmcs::client::CBoard { - public: - friend class SteeringInfantry; - explicit TopBoard( - SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, - int usb_pid = -1) - : CBoard(usb_pid) - , tf_(steeringInfantry.tf_) - , bmi088_(1000, 0.2, 0.0) - , gimbal_pitch_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/pitch") - , gimbal_left_friction_( - steeringInfantry, steeringInfantry_command, "/gimbal/left_friction") - , gimbal_right_friction_( - steeringInfantry, steeringInfantry_command, "/gimbal/right_friction") - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - gimbal_pitch_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("pitch_motor_zero_point").as_int()))); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(1.) - .set_reversed()); - - steeringInfantry.register_output( - "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); - steeringInfantry.register_output( - "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); - - bmi088_.set_coordinate_mapping([](double x, double y, double z) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise - // use a matrix. - - // Eigen::AngleAxisd pitch_link_to_bmi088_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(x, y, z); - }); - } - ~TopBoard() { - stop_handling_events(); - event_thread_.join(); - } - void update() { - bmi088_.update_status(); - Eigen::Quaterniond gimbal_bmi088_pose{ - bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; - - tf_->set_transform( - gimbal_bmi088_pose.conjugate()); - - *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); - *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); - - gimbal_pitch_motor_.update_status(); - gimbal_left_friction_.update_status(); - gimbal_right_friction_.update_status(); - - tf_->set_state( - gimbal_pitch_motor_.angle()); - } - - void command_update() { - uint16_t can_commands[4]; - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.add_can2_transmission( - 0x142, gimbal_pitch_motor_.generate_velocity_command( - gimbal_pitch_motor_.control_velocity())); - - transmit_buffer_.trigger_transmission(); - } - - private: - 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 == 0x203) { - gimbal_left_friction_.store_status(can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - if (can_id == 0x142) - gimbal_pitch_motor_.store_status(can_data); - } - 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); - } - OutputInterface& tf_; - - OutputInterface gimbal_yaw_velocity_bmi088_; - OutputInterface gimbal_pitch_velocity_bmi088_; - - device::Bmi088 bmi088_; - device::LkMotor gimbal_pitch_motor_; - device::DjiMotor gimbal_left_friction_; - device::DjiMotor gimbal_right_friction_; - - TransmitBuffer transmit_buffer_; - std::thread event_thread_; - }; - class BottomBoard final : private librmcs::client::CBoard { - public: - friend class SteeringInfantry; - - explicit BottomBoard( - SteeringInfantry& steeringInfantry, SteeringInfantryCommand& steeringInfantry_command, - int usb_pid = -1) - : librmcs::client::CBoard(usb_pid) - , imu_(1000, 0.2, 0.0) - , tf_(steeringInfantry.tf_) - , dr16_(steeringInfantry) - , gimbal_yaw_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/yaw") - , gimbal_bullet_feeder_( - steeringInfantry, steeringInfantry_command, "/gimbal/bullet_feeder") - , chassis_wheel_motors_( - {steeringInfantry, steeringInfantry_command, "/chassis/left_front_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/left_back_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_back_wheel"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_front_wheel"}) - , chassis_steer_motors_( - {steeringInfantry, steeringInfantry_command, "/chassis/left_front_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/left_back_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_back_steering"}, - {steeringInfantry, steeringInfantry_command, "/chassis/right_front_steering"}) - , supercap_(steeringInfantry, steeringInfantry_command) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - steeringInfantry.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; - }; - gimbal_yaw_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("yaw_motor_zero_point").as_int()))); - - gimbal_bullet_feeder_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M2006} - .enable_multi_turn_angle() - .set_reversed() - .set_reduction_ratio(19 * 2)); - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - - .set_reduction_ratio(11.) - .enable_multi_turn_angle() - .set_reversed()); - chassis_steer_motors_[0].configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("left_front_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[1].configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("left_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[2].configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("right_back_zero_point").as_int())) - .enable_multi_turn_angle()); - chassis_steer_motors_[3].configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .set_reversed() - .set_encoder_zero_point( - static_cast( - steeringInfantry.get_parameter("right_front_zero_point").as_int())) - .enable_multi_turn_angle()); - steeringInfantry.register_output( - "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); - } - ~BottomBoard() final { - stop_handling_events(); - event_thread_.join(); - } - void update() { - imu_.update_status(); - *chassis_yaw_velocity_imu_ = imu_.gy(); - supercap_.update_status(); - - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - for (auto& motor : chassis_steer_motors_) - motor.update_status(); - - dr16_.update_status(); - gimbal_yaw_motor_.update_status(); - tf_->set_state( - gimbal_yaw_motor_.angle()); - - gimbal_bullet_feeder_.update_status(); - } - void command_update() { - uint16_t batch_commands[4]; - for (int i = 0; i < 4; i++) - batch_commands[i] = chassis_wheel_motors_[i].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(batch_commands)); - transmit_buffer_.add_can1_transmission(0x1FF, gimbal_bullet_feeder_.generate_command()); - if (can_transmission_mode) { - batch_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission( - 0x1FE, std::bit_cast(batch_commands)); - transmit_buffer_.trigger_transmission(); - for (int i = 0; i < 4; i++) { - batch_commands[i] = chassis_steer_motors_[i].generate_command(); - } - transmit_buffer_.add_can2_transmission( - 0x1FE, std::bit_cast(batch_commands)); - transmit_buffer_.trigger_transmission(); - } else { - transmit_buffer_.add_can2_transmission( - 0x142, gimbal_yaw_motor_.generate_velocity_command( - gimbal_yaw_motor_.control_velocity() - imu_.gy())); - transmit_buffer_.trigger_transmission(); - } - can_transmission_mode = !can_transmission_mode; - } - - void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { - dr16_.store_status(uart_data, uart_data_length); - } - 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 == 0x201) - chassis_wheel_motors_[0].store_status(can_data); - else if (can_id == 0x202) - chassis_wheel_motors_[1].store_status(can_data); - else if (can_id == 0x203) - chassis_wheel_motors_[2].store_status(can_data); - else if (can_id == 0x204) - chassis_wheel_motors_[3].store_status(can_data); - else if (can_id == 0x205) - gimbal_bullet_feeder_.store_status(can_data); - else if (can_id == 0x300) - supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - if (can_id == 0x142) - gimbal_yaw_motor_.store_status(can_data); - else if (can_id == 0x205) - chassis_steer_motors_[0].store_status(can_data); - else if (can_id == 0x206) - chassis_steer_motors_[1].store_status(can_data); - else if (can_id == 0x207) - chassis_steer_motors_[2].store_status(can_data); - else if (can_id == 0x208) - chassis_steer_motors_[3].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 accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_accelerometer_status(x, y, z); - } - - void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { - imu_.store_gyroscope_status(x, y, z); - } - - private: - bool can_transmission_mode = true; - device::Bmi088 imu_; - OutputInterface& tf_; - OutputInterface powermeter_control_enabled_; - OutputInterface powermeter_charge_power_limit_; - - device::Dr16 dr16_; - device::LkMotor gimbal_yaw_motor_; - device::DjiMotor gimbal_bullet_feeder_; - device::DjiMotor chassis_wheel_motors_[4]; - device::DjiMotor chassis_steer_motors_[4]; - device::Supercap supercap_; - - librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; - OutputInterface referee_serial_; - OutputInterface chassis_yaw_velocity_imu_; - - librmcs::client::CBoard::TransmitBuffer transmit_buffer_; - std::thread event_thread_; - }; - - OutputInterface tf_; - - std::shared_ptr command_component_; - std::shared_ptr top_board_; - std::shared_ptr bottom_board_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; -}; - -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) \ No newline at end of file +// #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 "hardware/device/supercap.hpp" +// #include "librmcs/client/cboard.hpp" + +// namespace rmcs_core::hardware { + +// class SteeringInfantry +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// SteeringInfantry() +// : Node( +// get_component_name(), +// rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) +// , command_component_( +// create_partner_component( +// get_component_name() + "_command", *this)) { +// using namespace rmcs_description; +// register_output("/tf", tf_); + +// gimbal_calibrate_subscription_ = create_subscription( +// "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// gimbal_calibrate_subscription_callback(std::move(msg)); +// }); +// steers_calibrate_subscription_ = create_subscription( +// "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// steers_calibrate_subscription_callback(std::move(msg)); +// }); + +// top_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_top_board").as_int())); + +// bottom_board_ = std::make_unique( +// *this, *command_component_, +// static_cast(get_parameter("usb_pid_bottom_board").as_int())); + +// using namespace rmcs_description; +// tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); +// } +// ~SteeringInfantry() override = default; + +// void update() override { +// top_board_->update(); +// bottom_board_->update(); +// } + +// void command_update() { + +// top_board_->command_update(); +// bottom_board_->command_update(); +// } + +// private: +// void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New yaw offset: %ld", +// bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[gimbal calibration] New pitch offset: %ld", +// top_board_->gimbal_pitch_motor_.calibrate_zero_point()); +// } + +// void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { +// RCLCPP_INFO( +// get_logger(), "[steer calibration] New left front offset: %d", +// bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[steer calibration] New left back offset: %d", +// bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[steer calibration] New right back offset: %d", +// bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); +// RCLCPP_INFO( +// get_logger(), "[steer calibration] New right front offset: %d", +// bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); +// } +// class SteeringInfantryCommand : public rmcs_executor::Component { +// public: +// explicit SteeringInfantryCommand(SteeringInfantry& steeringInfantry) +// : steeringInfantry(steeringInfantry) {} + +// void update() override { steeringInfantry.command_update(); } + +// SteeringInfantry& steeringInfantry; +// }; + +// class TopBoard final : private librmcs::client::CBoard { +// public: +// friend class SteeringInfantry; +// explicit TopBoard( +// SteeringInfantry& steeringInfantry, SteeringInfantryCommand& +// steeringInfantry_command, int usb_pid = -1) : CBoard(usb_pid) , +// tf_(steeringInfantry.tf_) , bmi088_(1000, 0.2, 0.0) , +// gimbal_pitch_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/pitch") , +// gimbal_left_friction_( +// steeringInfantry, steeringInfantry_command, "/gimbal/left_friction") +// , gimbal_right_friction_( +// steeringInfantry, steeringInfantry_command, "/gimbal/right_friction") +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { +// gimbal_pitch_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("pitch_motor_zero_point").as_int()))); + +// gimbal_left_friction_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); +// gimbal_right_friction_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reduction_ratio(1.) +// .set_reversed()); + +// steeringInfantry.register_output( +// "/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); +// steeringInfantry.register_output( +// "/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + +// bmi088_.set_coordinate_mapping([](double x, double y, double z) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise +// // use a matrix. + +// // Eigen::AngleAxisd pitch_link_to_bmi088_link{ +// // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; +// // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * Eigen::Vector3d{1, 2, +// 3}; +// // std::cout << mapping << std::endl; + +// return std::make_tuple(x, y, z); +// }); +// } +// ~TopBoard() { +// stop_handling_events(); +// event_thread_.join(); +// } +// void update() { +// bmi088_.update_status(); +// Eigen::Quaterniond gimbal_bmi088_pose{ +// bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + +// tf_->set_transform( +// gimbal_bmi088_pose.conjugate()); + +// *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); +// *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + +// gimbal_pitch_motor_.update_status(); +// gimbal_left_friction_.update_status(); +// gimbal_right_friction_.update_status(); + +// tf_->set_state( +// gimbal_pitch_motor_.angle()); +// } + +// void command_update() { +// uint16_t can_commands[4]; +// can_commands[2] = gimbal_left_friction_.generate_command(); +// can_commands[3] = gimbal_right_friction_.generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + +// transmit_buffer_.add_can2_transmission( +// 0x142, gimbal_pitch_motor_.generate_velocity_command( +// gimbal_pitch_motor_.control_velocity())); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// 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 == 0x203) { +// gimbal_left_friction_.store_status(can_data); +// } else if (can_id == 0x204) { +// gimbal_right_friction_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; +// if (can_id == 0x142) +// gimbal_pitch_motor_.store_status(can_data); +// } +// 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); +// } +// OutputInterface& tf_; + +// OutputInterface gimbal_yaw_velocity_bmi088_; +// OutputInterface gimbal_pitch_velocity_bmi088_; + +// device::Bmi088 bmi088_; +// device::LkMotor gimbal_pitch_motor_; +// device::DjiMotor gimbal_left_friction_; +// device::DjiMotor gimbal_right_friction_; + +// TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// }; +// class BottomBoard final : private librmcs::client::CBoard { +// public: +// friend class SteeringInfantry; + +// explicit BottomBoard( +// SteeringInfantry& steeringInfantry, SteeringInfantryCommand& +// steeringInfantry_command, int usb_pid = -1) : librmcs::client::CBoard(usb_pid) , +// imu_(1000, 0.2, 0.0) , tf_(steeringInfantry.tf_) , dr16_(steeringInfantry) , +// gimbal_yaw_motor_(steeringInfantry, steeringInfantry_command, "/gimbal/yaw") , +// gimbal_bullet_feeder_( +// steeringInfantry, steeringInfantry_command, "/gimbal/bullet_feeder") +// , chassis_wheel_motors_( +// {steeringInfantry, steeringInfantry_command, "/chassis/left_front_wheel"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/left_back_wheel"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/right_back_wheel"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/right_front_wheel"}) +// , chassis_steer_motors_( +// {steeringInfantry, steeringInfantry_command, "/chassis/left_front_steering"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/left_back_steering"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/right_back_steering"}, +// {steeringInfantry, steeringInfantry_command, "/chassis/right_front_steering"}) +// , supercap_(steeringInfantry, steeringInfantry_command) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// steeringInfantry.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; +// }; +// gimbal_yaw_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10}.set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("yaw_motor_zero_point").as_int()))); + +// gimbal_bullet_feeder_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M2006} +// .enable_multi_turn_angle() +// .set_reversed() +// .set_reduction_ratio(19 * 2)); + +// for (auto& motor : chassis_wheel_motors_) +// motor.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} + +// .set_reduction_ratio(11.) +// .enable_multi_turn_angle() +// .set_reversed()); +// chassis_steer_motors_[0].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("left_front_zero_point").as_int())) +// .enable_multi_turn_angle()); +// chassis_steer_motors_[1].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("left_back_zero_point").as_int())) +// .enable_multi_turn_angle()); +// chassis_steer_motors_[2].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("right_back_zero_point").as_int())) +// .enable_multi_turn_angle()); +// chassis_steer_motors_[3].configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .set_reversed() +// .set_encoder_zero_point( +// static_cast( +// steeringInfantry.get_parameter("right_front_zero_point").as_int())) +// .enable_multi_turn_angle()); +// steeringInfantry.register_output( +// "/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); +// } +// ~BottomBoard() final { +// stop_handling_events(); +// event_thread_.join(); +// } +// void update() { +// imu_.update_status(); +// *chassis_yaw_velocity_imu_ = imu_.gy(); +// supercap_.update_status(); + +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); +// for (auto& motor : chassis_steer_motors_) +// motor.update_status(); + +// dr16_.update_status(); +// gimbal_yaw_motor_.update_status(); +// tf_->set_state( +// gimbal_yaw_motor_.angle()); + +// gimbal_bullet_feeder_.update_status(); +// } +// void command_update() { +// uint16_t batch_commands[4]; +// for (int i = 0; i < 4; i++) +// batch_commands[i] = chassis_wheel_motors_[i].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, +// std::bit_cast(batch_commands)); +// transmit_buffer_.add_can1_transmission(0x1FF, +// gimbal_bullet_feeder_.generate_command()); if (can_transmission_mode) { +// batch_commands[3] = supercap_.generate_command(); +// transmit_buffer_.add_can1_transmission( +// 0x1FE, std::bit_cast(batch_commands)); +// transmit_buffer_.trigger_transmission(); +// for (int i = 0; i < 4; i++) { +// batch_commands[i] = chassis_steer_motors_[i].generate_command(); +// } +// transmit_buffer_.add_can2_transmission( +// 0x1FE, std::bit_cast(batch_commands)); +// transmit_buffer_.trigger_transmission(); +// } else { +// transmit_buffer_.add_can2_transmission( +// 0x142, gimbal_yaw_motor_.generate_velocity_command( +// gimbal_yaw_motor_.control_velocity() - imu_.gy())); +// transmit_buffer_.trigger_transmission(); +// } +// can_transmission_mode = !can_transmission_mode; +// } + +// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override +// { +// dr16_.store_status(uart_data, uart_data_length); +// } +// 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 == 0x201) +// chassis_wheel_motors_[0].store_status(can_data); +// else if (can_id == 0x202) +// chassis_wheel_motors_[1].store_status(can_data); +// else if (can_id == 0x203) +// chassis_wheel_motors_[2].store_status(can_data); +// else if (can_id == 0x204) +// chassis_wheel_motors_[3].store_status(can_data); +// else if (can_id == 0x205) +// gimbal_bullet_feeder_.store_status(can_data); +// else if (can_id == 0x300) +// supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; +// if (can_id == 0x142) +// gimbal_yaw_motor_.store_status(can_data); +// else if (can_id == 0x205) +// chassis_steer_motors_[0].store_status(can_data); +// else if (can_id == 0x206) +// chassis_steer_motors_[1].store_status(can_data); +// else if (can_id == 0x207) +// chassis_steer_motors_[2].store_status(can_data); +// else if (can_id == 0x208) +// chassis_steer_motors_[3].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 accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_accelerometer_status(x, y, z); +// } + +// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { +// imu_.store_gyroscope_status(x, y, z); +// } + +// private: +// bool can_transmission_mode = true; +// device::Bmi088 imu_; +// OutputInterface& tf_; +// OutputInterface powermeter_control_enabled_; +// OutputInterface powermeter_charge_power_limit_; + +// device::Dr16 dr16_; +// device::LkMotor gimbal_yaw_motor_; +// device::DjiMotor gimbal_bullet_feeder_; +// device::DjiMotor chassis_wheel_motors_[4]; +// device::DjiMotor chassis_steer_motors_[4]; +// device::Supercap supercap_; + +// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; +// OutputInterface referee_serial_; +// OutputInterface chassis_yaw_velocity_imu_; + +// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; +// std::thread event_thread_; +// }; + +// OutputInterface tf_; + +// std::shared_ptr command_component_; +// std::shared_ptr top_board_; +// std::shared_ptr bottom_board_; + +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; +// rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; +// }; + +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::SteeringInfantry, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp index 8c29292f..6a50d83b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/tunnel_infantry.cpp @@ -1,305 +1,308 @@ -#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 "hardware/device/supercap.hpp" - -namespace rmcs_core::hardware { - -class TunnelInfantry - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::client::CBoard { -public: - TunnelInfantry() - : 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()) - , infantry_command_( - create_partner_component(get_component_name() + "_command", *this)) - , chassis_wheel_motors_( - {*this, *infantry_command_, "/chassis/left_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_front_wheel"}, - {*this, *infantry_command_, "/chassis/right_back_wheel"}, - {*this, *infantry_command_, "/chassis/left_back_wheel"}) - , supercap_(*this, *infantry_command_) - , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") - , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") - , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") - , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") - , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") - , dr16_{*this} - , bmi088_(1000, 0.2, 0.0) - , transmit_buffer_(*this, 32) - , event_thread_([this]() { handle_events(); }) { - - for (auto& motor : chassis_wheel_motors_) - motor.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .set_reduction_ratio(13.) - .enable_multi_turn_angle()); - - gimbal_yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::GM6020} - .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())) - .set_reversed()); - - gimbal_left_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); - gimbal_right_friction_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reversed() - .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) { - // Get the mapping with the following code. - // The rotation angle must be an exact multiple of 90 degrees, otherwise use a matrix. - - // Eigen::AngleAxisd pitch_link_to_imu_link{ - // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; - // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; - // std::cout << mapping << std::endl; - - return std::make_tuple(-y, x, z); - }); - - using namespace rmcs_description; - tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); - - constexpr double gimbal_center_height = 0.32059; - constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; - tf_->set_transform( - Eigen::Translation3d{0, 0, gimbal_center_height}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - tf_->set_transform( - Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); - - 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; - }; - } - - ~TunnelInfantry() override { - stop_handling_events(); - event_thread_.join(); - } - - void update() override { - update_motors(); - update_imu(); - dr16_.update_status(); - supercap_.update_status(); - } - - void command_update() { - uint16_t can_commands[4]; - - can_commands[0] = gimbal_yaw_motor_.generate_command(); - can_commands[1] = gimbal_pitch_motor_.generate_command(); - can_commands[2] = 0; - can_commands[3] = supercap_.generate_command(); - transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); - - can_commands[0] = chassis_wheel_motors_[0].generate_command(); - can_commands[1] = chassis_wheel_motors_[1].generate_command(); - can_commands[2] = chassis_wheel_motors_[2].generate_command(); - can_commands[3] = chassis_wheel_motors_[3].generate_command(); - transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); - - can_commands[0] = 0; - can_commands[1] = gimbal_bullet_feeder_.generate_command(); - can_commands[2] = gimbal_left_friction_.generate_command(); - can_commands[3] = gimbal_right_friction_.generate_command(); - transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - - transmit_buffer_.trigger_transmission(); - } - -private: - void update_motors() { - using namespace rmcs_description; - for (auto& motor : chassis_wheel_motors_) - motor.update_status(); - tf_->set_state(chassis_wheel_motors_[0].angle()); - tf_->set_state(chassis_wheel_motors_[1].angle()); - tf_->set_state(chassis_wheel_motors_[2].angle()); - tf_->set_state(chassis_wheel_motors_[3].angle()); - - 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: %d", - 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 == 0x201) { - auto& motor = chassis_wheel_motors_[0]; - motor.store_status(can_data); - } else if (can_id == 0x202) { - auto& motor = chassis_wheel_motors_[1]; - motor.store_status(can_data); - } else if (can_id == 0x203) { - auto& motor = chassis_wheel_motors_[2]; - motor.store_status(can_data); - } else if (can_id == 0x204) { - auto& motor = chassis_wheel_motors_[3]; - motor.store_status(can_data); - } else if (can_id == 0x205) { - gimbal_yaw_motor_.store_status(can_data); - } else if (can_id == 0x206) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x300) { - supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] - return; - - if (can_id == 0x142) { - gimbal_pitch_motor_.store_status(can_data); - } else if (can_id == 0x202) { - gimbal_bullet_feeder_.store_status(can_data); - } else if (can_id == 0x203) { - gimbal_left_friction_.store_status(can_data); - } else if (can_id == 0x204) { - gimbal_right_friction_.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 InfantryCommand : public rmcs_executor::Component { - public: - explicit InfantryCommand(TunnelInfantry& infantry) - : infantry_(infantry) {} - - void update() override { infantry_.command_update(); } - - TunnelInfantry& infantry_; - }; - std::shared_ptr infantry_command_; - - rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; - - device::DjiMotor chassis_wheel_motors_[4]; - device::Supercap supercap_; - - device::DjiMotor 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::TunnelInfantry, rmcs_executor::Component) \ No newline at end of file +// #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 "hardware/device/supercap.hpp" + +// namespace rmcs_core::hardware { + +// class TunnelInfantry +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::client::CBoard { +// public: +// TunnelInfantry() +// : 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()) , infantry_command_( +// create_partner_component(get_component_name() + "_command", +// *this)) +// , chassis_wheel_motors_( +// {*this, *infantry_command_, "/chassis/left_front_wheel"}, +// {*this, *infantry_command_, "/chassis/right_front_wheel"}, +// {*this, *infantry_command_, "/chassis/right_back_wheel"}, +// {*this, *infantry_command_, "/chassis/left_back_wheel"}) +// , supercap_(*this, *infantry_command_) +// , gimbal_yaw_motor_(*this, *infantry_command_, "/gimbal/yaw") +// , gimbal_pitch_motor_(*this, *infantry_command_, "/gimbal/pitch") +// , gimbal_left_friction_(*this, *infantry_command_, "/gimbal/left_friction") +// , gimbal_right_friction_(*this, *infantry_command_, "/gimbal/right_friction") +// , gimbal_bullet_feeder_(*this, *infantry_command_, "/gimbal/bullet_feeder") +// , dr16_{*this} +// , bmi088_(1000, 0.2, 0.0) +// , transmit_buffer_(*this, 32) +// , event_thread_([this]() { handle_events(); }) { + +// for (auto& motor : chassis_wheel_motors_) +// motor.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .set_reduction_ratio(13.) +// .enable_multi_turn_angle()); + +// gimbal_yaw_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::GM6020} +// .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())) +// .set_reversed()); + +// gimbal_left_friction_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(1.)); +// gimbal_right_friction_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::M3508} +// .set_reversed() +// .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) { +// // Get the mapping with the following code. +// // The rotation angle must be an exact multiple of 90 degrees, otherwise use a +// matrix. + +// // Eigen::AngleAxisd pitch_link_to_imu_link{ +// // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; +// // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; +// // std::cout << mapping << std::endl; + +// return std::make_tuple(-y, x, z); +// }); + +// using namespace rmcs_description; +// tf_->set_transform(Eigen::Translation3d{0.06603, 0.0, 0.082}); + +// constexpr double gimbal_center_height = 0.32059; +// constexpr double wheel_distance_x = 0.15897, wheel_distance_y = 0.15897; +// tf_->set_transform( +// Eigen::Translation3d{0, 0, gimbal_center_height}); +// tf_->set_transform( +// Eigen::Translation3d{wheel_distance_x / 2, wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{-wheel_distance_x / 2, wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{-wheel_distance_x / 2, -wheel_distance_y / 2, 0}); +// tf_->set_transform( +// Eigen::Translation3d{wheel_distance_x / 2, -wheel_distance_y / 2, 0}); + +// 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; +// }; +// } + +// ~TunnelInfantry() override { +// stop_handling_events(); +// event_thread_.join(); +// } + +// void update() override { +// update_motors(); +// update_imu(); +// dr16_.update_status(); +// supercap_.update_status(); +// } + +// void command_update() { +// uint16_t can_commands[4]; + +// can_commands[0] = gimbal_yaw_motor_.generate_command(); +// can_commands[1] = gimbal_pitch_motor_.generate_command(); +// can_commands[2] = 0; +// can_commands[3] = supercap_.generate_command(); +// transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(can_commands)); + +// can_commands[0] = chassis_wheel_motors_[0].generate_command(); +// can_commands[1] = chassis_wheel_motors_[1].generate_command(); +// can_commands[2] = chassis_wheel_motors_[2].generate_command(); +// can_commands[3] = chassis_wheel_motors_[3].generate_command(); +// transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(can_commands)); + +// transmit_buffer_.add_can2_transmission(0x142, gimbal_pitch_motor_.generate_command()); + +// can_commands[0] = 0; +// can_commands[1] = gimbal_bullet_feeder_.generate_command(); +// can_commands[2] = gimbal_left_friction_.generate_command(); +// can_commands[3] = gimbal_right_friction_.generate_command(); +// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); + +// transmit_buffer_.trigger_transmission(); +// } + +// private: +// void update_motors() { +// using namespace rmcs_description; +// for (auto& motor : chassis_wheel_motors_) +// motor.update_status(); +// tf_->set_state(chassis_wheel_motors_[0].angle()); +// tf_->set_state(chassis_wheel_motors_[1].angle()); +// tf_->set_state(chassis_wheel_motors_[2].angle()); +// tf_->set_state(chassis_wheel_motors_[3].angle()); + +// 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: %d", +// 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 == 0x201) { +// auto& motor = chassis_wheel_motors_[0]; +// motor.store_status(can_data); +// } else if (can_id == 0x202) { +// auto& motor = chassis_wheel_motors_[1]; +// motor.store_status(can_data); +// } else if (can_id == 0x203) { +// auto& motor = chassis_wheel_motors_[2]; +// motor.store_status(can_data); +// } else if (can_id == 0x204) { +// auto& motor = chassis_wheel_motors_[3]; +// motor.store_status(can_data); +// } else if (can_id == 0x205) { +// gimbal_yaw_motor_.store_status(can_data); +// } else if (can_id == 0x206) { +// gimbal_pitch_motor_.store_status(can_data); +// } else if (can_id == 0x300) { +// supercap_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] +// return; + +// if (can_id == 0x142) { +// gimbal_pitch_motor_.store_status(can_data); +// } else if (can_id == 0x202) { +// gimbal_bullet_feeder_.store_status(can_data); +// } else if (can_id == 0x203) { +// gimbal_left_friction_.store_status(can_data); +// } else if (can_id == 0x204) { +// gimbal_right_friction_.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 InfantryCommand : public rmcs_executor::Component { +// public: +// explicit InfantryCommand(TunnelInfantry& infantry) +// : infantry_(infantry) {} + +// void update() override { infantry_.command_update(); } + +// TunnelInfantry& infantry_; +// }; +// std::shared_ptr infantry_command_; + +// rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + +// device::DjiMotor chassis_wheel_motors_[4]; +// device::Supercap supercap_; + +// device::DjiMotor 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::TunnelInfantry, rmcs_executor::Component) \ No newline at end of file From db05722e57805ed5060c63fed2b7f68a522e8574 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Fri, 13 Mar 2026 23:50:29 +0800 Subject: [PATCH 36/45] tested manager passed --- .../config/dart-filling-test.yaml | 33 +- .../rmcs_bringup/config/dart-guidance.yaml | 88 ++- .../rmcs_bringup/config/dart-launch-full.yaml | 74 ++ .../src/rmcs_bringup/config/dart-launch.yaml | 68 ++ rmcs_ws/src/rmcs_bringup/config/dart.yaml | 74 +- rmcs_ws/src/rmcs_core/plugins.xml | 21 + .../src/controller/dart/dart_filling_test.cpp | 239 ++++--- .../src/controller/dart/dart_setting.cpp | 58 ++ .../src/controller/dart/dart_settings.cpp | 19 +- .../src/controller/dart/launch_controller.cpp | 634 +++++++++--------- .../controller/dart/launch_controller_v2.cpp | 489 ++++++++++++++ .../dart/launch_controller_v2_full.cpp | 359 ++++++++++ .../src/controller/dart/launch_setting.cpp | 116 ++++ .../rmcs_core/src/hardware/catapult_dart.cpp | 26 +- .../src/hardware/catapult_dart_librmcs_v3.cpp | 284 ++++---- .../src/hardware/catapult_dart_v3_full.cpp | 426 ++++++++++++ .../src/hardware/catapult_dart_v3_lk.cpp | 538 +++++++++++++++ .../src/hardware/catapult_dart_v4.cpp | 426 ++++++++++++ .../hardware/dart_filling_test_hardware.cpp | 71 +- .../rmcs_core/src/hardware/device/dr16.hpp | 280 ++++++-- .../src/hardware/device/force_sensor.hpp | 30 +- .../src/hardware/device/hojo_board.hpp | 96 --- .../src/hardware/device/lifting_left.hpp | 175 ----- .../src/hardware/device/lifting_right.hpp | 176 ----- .../src/hardware/device/limiting_servo.hpp | 622 ----------------- rmcs_ws/src/rmcs_dart_guidance | 2 +- .../include/rmcs_executor/src/component.cpp | 7 + .../include/rmcs_executor/src/executor.hpp | 185 +++++ .../include/rmcs_executor/src/main.cpp | 73 ++ .../src/predefined_msg_provider.hpp | 23 + .../include/rmcs_msgs/dart_slider_status.hpp | 11 + 31 files changed, 3851 insertions(+), 1872 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp create mode 100644 rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/component.cpp create mode 100644 rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp create mode 100644 rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp create mode 100644 rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_slider_status.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml index 629c850f..88c9baee 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml @@ -2,30 +2,23 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::DartFillingTestHardware -> catapult_dart - - rmcs_core::controller::dart::DartFillingTest -> launch_controller - # - rmcs_core::controller::dart::Test -> actual_speed_and_control_torque_test_node + - rmcs_core::hardware::CatapultDartV3Full -> catapult_dart + - rmcs_core::controller::dart::DartFillingTest -> launch_controller catapult_dart: ros__parameters: - usb_pid: -1 + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 launch_controller: ros__parameters: - lifting_up_angle_left: 0x0656 - lifting_down_angle_left: 0x0DE3 - lifting_up_angle_right: 0x08C7 - lifting_down_angle_right: 0x0C54 - trigger_free_angle: 0x0E2A - trigger_lock_angle: 0x0FC1 - limiting_wait_time: 0x0010 - limiting_free_angle_: 0x0001 - limiting_lock_angle_: 0x0050 + # Trigger servo: PWM value 0.0–1.0 + # PWMServo range: 0.5ms–2.5ms in a 20ms period + # 0.0 → 0.5ms (full CCW), 0.5 → 1.5ms (center), 1.0 → 2.5ms (full CW) + trigger_free_value: 0.75 # tune to match physical free position + trigger_lock_value: 0.63 # tune to match physical lock position - -# actual_speed_and_control_torque_test_node: -# ros__parameters: -# lifting_up_angle_left: 0x0656 -# lifting_down_angle_left: 0x0DE3 -# lifting_up_angle_right: 0x08C7 -# lifting_down_angle_right: 0x0C54 \ No newline at end of file + # Motor torque limits (N·m) + max_torque: 1.0 + max_belt_torque: 5.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 4f3cd01a..ee4f271e 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -2,60 +2,54 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_dart_guidance::DartVisionCore -> dart_vision_core - - rmcs_dart_guidance::DebugDisplayComponent -> debug_display - - rmcs_dart_guidance::ImagePublisher -> image_publisher - - rmcs_dart_guidance::ImagePublisher -> camera_parameter_image_publisher - - rmcs_dart_guidance::CameraCalibrationComponent -> dart_vision_calibration + - rmcs_core::hardware::CatapultDartV4 -> dart_hardware + - rmcs_dart_guidance::manager::DartManager -> dart_manager + - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge + - rmcs_core::controller::dart::DartLaunchSetting -> launch_setting + - rmcs_core::controller::dart::DartSettingController -> dart_setting + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller -dart_vision_core: +dart_hardware: ros__parameters: - invert_image: false - exposure_time: 5000 - gain: 11.0 - L_H: 0.0 - L_S: 0.0 - L_V: 200.0 - U_H: 180.0 - U_S: 30.0 - U_V: 255.0 - enable_image_saving: false - image_save_directory: "./saved_images" - image_save_interval_ms: 1000 - save_raw_image: true - save_processed_image: false + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 -debug_display: +dart_manager: ros__parameters: - raw_image_topic: "/dart_guidance/raw_image" - processed_image_topic: "/dart_guidance/processed_image" - target_topic: "/dart_guidance/target_position" - display_raw: false - display_processed: true - max_fps: 30 - window_scale: 1.0 - save_on_error: false - save_directory: "./debug_saved" + max_transform_rate: 15.0 + manual_force_scale: 15.0 -image_publisher: +remote_cmd_bridge: ros__parameters: - Interface_name: "/dart_guidance/camera/display_image" - topic_name: "/dart_guidance/processed_image" - publish_freq: 15.0 - image_type: "mono8" + joystick_sensitivity: 0.01 -camera_parameter_image_publisher: +launch_setting: ros__parameters: - Interface_name: "/dart_guidance/camera/camera_image" - topic_name: "/dart_guidance/camera/camera_image" - publish_freq: 30.0 - image_type: "bgr8" + belt_velocity: 15.0 + sync_coefficient: 0.1 + max_control_torque: 15.0 + trigger_free_value: 0.75 + trigger_lock_value: 0.63 + b_kp: 1.0 + b_ki: 0.0 + b_kd: 0.0 -dart_vision_calibration: +dart_setting: ros__parameters: - image_topic: "/dart_guidance/camera/camera_image" - board_width: 7 - board_height: 7 - square_size: 0.006 - save_path: "./calibration_results" - enabled: true \ No newline at end of file + yaw_kp: 1.0 + yaw_ki: 0.0 + yaw_kd: 0.0 + pitch_kp: 1.0 + pitch_ki: 0.0 + pitch_kd: 0.0 + +force_screw_velocity_pid_controller: + ros__parameters: + measurement: /dart/force_screw/velocity + setpoint: /force/control/velocity + control: /dart/force_screw/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml new file mode 100644 index 00000000..bf3959f6 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml @@ -0,0 +1,74 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware + - rmcs_core::controller::dart::DartLaunchControllerV2Full -> launch_controller + - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + +broadcaster: + ros__parameters: + forward_list: + - /dart/drive_belt/left/control_torque + - /dart/drive_belt/left/velocity + - /dart/drive_belt/right/control_torque + - /dart/drive_belt/right/velocity + - /dart/lifting_left/angle + - /dart/lifting_right/angle + - /force/sensor/average + +dart_hardware: + ros__parameters: + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 + +launch_controller: + ros__parameters: + belt_velocity: 10.0 + sync_coefficient: 0.1 + max_control_torque: 15.0 + # Trigger servo PWM value 0.0–1.0 (from dart-filling-test calibration) + trigger_free_value: 0.75 + trigger_lock_value: 0.66 + # Duration to keep trigger free after launch (ms) + trigger_free_duration_ms: 300 + # Time to wait after platform move before belt resets (ms) + lifting_settle_ms: 500 + # Duration to keep limiting servo open for dart feeding (ms) + limiting_open_duration_ms: 2000 + # Lifting motor angles in DEGREES — will be converted to radians in code. + # 待标定: 上电后将平台移到 up 位置，记录角度填入 *_up_deg； + # 移到 down 位置记录填入 *_down_deg。 + lifting_up_angle_left_deg: 0.0 + lifting_down_angle_left_deg: 90.0 + lifting_up_angle_right_deg: 0.0 + lifting_down_angle_right_deg: 90.0 + # Limiting servo (TriggerServo UART, uint16_t hex) + limiting_free_angle_: 0x0001 + limiting_lock_angle_: 0x0050 + +dart_settings: + ros__parameters: + is_auto_pitch_control_mode: 0 + is_auto_force_control_mode: 0 + max_transform_rate: 5.0 + pitch_angle_kp: 1.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + force_control_kp: 1.0 + force_control_ki: 0.0 + force_control_kd: 0.0 + +force_screw_velocity_pid_controller: + ros__parameters: + measurement: /dart/force_screw_motor/velocity + setpoint: /force/control/velocity + control: /dart/force_screw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml new file mode 100644 index 00000000..7a4b9ac3 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml @@ -0,0 +1,68 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDartV3Full -> dart_hardware + - rmcs_core::controller::dart::DartLaunchControllerV2 -> launch_controller + - rmcs_core::controller::dart::DartSettings -> dart_settings + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller + +dart_hardware: + ros__parameters: + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 + +launch_controller: + ros__parameters: + belt_velocity: 15.0 + sync_coefficient: 0.5 + max_control_torque: 15.0 + b_kp: 1.0 + b_ki: 0.0 + b_kd: 0.0 + # Trigger servo PWM value 0.0–1.0 (from dart-filling-test calibration) + trigger_free_value: 0.75 + trigger_lock_value: 0.63 + +dart_settings: + ros__parameters: + # 0 = manual joystick, 1 = auto PID + is_auto_pitch_control_mode: 0 + is_auto_force_control_mode: 0 + max_transform_rate: 5.0 + pitch_angle_kp: 0.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + force_control_kp: 0.001 + force_control_ki: 0.0 + force_control_kd: 0.0 + +force_screw_velocity_pid_controller: + ros__parameters: + measurement: /dart/force_screw_motor/velocity + setpoint: /force/control/velocity + control: /dart/force_screw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +yaw_velocity_pid_controller: + ros__parameters: + measurement: /dart/yaw_motor/velocity + setpoint: /yaw/control/velocity + control: /dart/yaw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +pitch_velocity_pid_controller: + ros__parameters: + measurement: /dart/pitch_motor/velocity + setpoint: /pitch/control/velocity + control: /dart/pitch_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml index 66bf9e34..514b50ec 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart.yaml @@ -2,7 +2,7 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::CatapultDart -> dart_hardware + - rmcs_core::hardware::CatapultDartV3Full -> dart_hardware - rmcs_core::controller::dart::DartLaunchController -> launch_controller - rmcs_core::controller::dart::DartSettings -> dart_settings - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller @@ -13,7 +13,7 @@ rmcs_executor: broadcaster: ros__parameters: forward_list: - - /dart/force_screw_motor/control_velocity + - /force/control/velocity - /dart/force_screw_motor/velocity - /dart/drive_belt/left/control_torque - /dart/drive_belt/left/velocity @@ -22,29 +22,9 @@ broadcaster: dart_hardware: ros__parameters: - usb_pid: -1 - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -servo_test: - ros__parameters: - trigger_free_angle: 0x0E2A - trigger_lock_angle: 0x0FC1 - lifting_up_angle_left: 0x073E - lifting_middle_angle_left: 0x073E - lifting_down_angle_left: 0x0D18 - lifting_up_angle_right: 0x11DB - lifting_middle_angle_right: 0x11DB - lifting_down_angle_right: 0x0C52 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /dart/pitch_motor/velocity - setpoint: /pitch/control/velocity - control: /dart/pitch_motor/control_torque - kp: 0.0 - ki: 0.0 - kd: 0.0 + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 launch_controller: ros__parameters: @@ -54,18 +34,39 @@ launch_controller: b_ki: 0.0 b_kd: 0.0 max_control_torque: 15.0 - trigger_free_angle: 0x0E2A - trigger_lock_angle: 0x0FC1 + # Trigger servo: PWM value 0.0–1.0 (PWMServo: 0.5ms–2.5ms in 20ms period) + trigger_free_value: 0.6 + trigger_lock_value: 0.4 + # Lifting UART servo angles + lifting_up_angle_left: 0x0656 + lifting_down_angle_left: 0x0DE3 + lifting_up_angle_right: 0x08C7 + lifting_down_angle_right: 0x0C54 + # Limiting servo angles + limiting_wait_time: 16 + limiting_free_angle_: 0x0001 + limiting_lock_angle_: 0x0050 dart_settings: ros__parameters: - log_enable: true - screw_max_velocity: 10.0 + # 0 = manual joystick, 1 = auto PID + is_auto_pitch_control_mode: 0 + is_auto_force_control_mode: 0 + max_transform_rate: 5.0 + # Pitch angle PID (used when is_auto_pitch_control_mode = 1) + pitch_angle_kp: 0.0 + pitch_angle_ki: 0.0 + pitch_angle_kd: 0.0 + # Force control PID (used when is_auto_force_control_mode = 1) + force_control_kp: 0.001 + force_control_ki: 0.0 + force_control_kd: 0.0 +# dart_settings outputs /force/control/velocity as the velocity setpoint for the screw motor force_screw_velocity_pid_controller: ros__parameters: measurement: /dart/force_screw_motor/velocity - setpoint: /dart/force_screw_motor/control_velocity + setpoint: /force/control/velocity control: /dart/force_screw_motor/control_torque kp: 1.0 ki: 0.0 @@ -80,14 +81,11 @@ yaw_velocity_pid_controller: ki: 0.0 kd: 0.0 -force_pid_controller: +pitch_velocity_pid_controller: ros__parameters: - measurement: /dart/force_control_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_control_motor/control_torque - kp: 1.0 - setpoint: /dart/yaw_motor/control_velocity - control: /dart/yaw_motor/control_torque + measurement: /dart/pitch_motor/velocity + setpoint: /pitch/control/velocity + control: /dart/pitch_motor/control_torque kp: 0.0 ki: 0.0 - kd: 0.0 \ No newline at end of file + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index e852e681..23162a9a 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -131,4 +131,25 @@ Test plugin. + + Full-featured catapult dart hardware with librmcs v3 API. + + + Catapult dart hardware v3 with LK4005 lifting motors (CAN1 0x141/0x142). + + + Launch controller Phase 1: belt + trigger only, no lifting/limiting. + + + Launch controller Phase 2: full filling sequence with LK4005 lifting + limiting servo. + + + Catapult dart hardware v4: force_screw path renamed to /dart/force_screw for DartManager compatibility. + + + Launch setting controller: subscribes to DartManager belt/trigger commands, outputs belt torques and trigger servo value. + + + Dart setting controller: subscribes to RemoteCommandBridge delta commands, outputs yaw/pitch/force control velocities. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp index 396972de..690b7c0b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp @@ -1,5 +1,5 @@ +#include #include -#include #include #include #include @@ -7,186 +7,185 @@ #include #include #include -#include /* -launch controls +DartFillingTest — 新方案综合验证控制器 键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } +双下 / UNKNOWN：全部停止 + +左拨杆中：调整角度模式 + 左摇杆 Y → yaw 电机力矩 + 右摇杆 X → pitch 电机力矩 + 右摇杆 Y → 传送带电机力矩（左右同步） + 右拨杆下拨再回中：降下抬升平台 + 右拨杆上拨再回中：抬起抬升平台 + +左拨杆上：发射/弹仓控制模式 + 右摇杆 X → 螺旋推力电机力矩 + 右拨杆下拨再回中：锁定扳机 + 限位舵机锁定 + 右拨杆上拨再回中：释放扳机 + 限位舵机释放 */ namespace rmcs_core::controller::dart { + class DartFillingTest : public rmcs_executor::Component , public rclcpp::Node { public: DartFillingTest() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , timer_interval_ms_(10) , logger_(get_logger()) { - launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); - launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); + // Trigger servo: PWM value (double 0.0–1.0) + trigger_free_value_ = get_parameter("trigger_free_value").as_double(); + trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); - limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); - limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); - limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); + max_torque_ = get_parameter("max_torque").as_double(); + max_belt_torque_ = get_parameter("max_belt_torque").as_double(); register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); - register_input("/dart/lifting_left/current_angle", lifting_angle_left_); - register_input("/dart/lifting_right/current_angle", lifting_angle_right_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - register_output("/dart/limiting_servo/control_angle", limiting_control_angle); - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); + // Trigger servo: PWM double value + register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); - register_output("/dart/filling/stage", filling_stage_); + // Motor torque outputs (direct control, no PID needed in test) + register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); + register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); + register_output("/dart/force_screw_motor/control_torque", force_screw_torque_, 0.0); + register_output("/dart/drive_belt/left/control_torque", belt_left_torque_, 0.0); + register_output("/dart/drive_belt/right/control_torque", belt_right_torque_, 0.0); + + register_output("/dart/filling/stage", filling_stage_); + register_output("/force/sensor/average", average_force_); timer_ = this->create_wall_timer( std::chrono::milliseconds(timer_interval_ms_), [this] { timer_callback(); }); - } void update() override { using namespace rmcs_msgs; - if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) - && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { - *launch_stage_ = DartLaunchStages::DISABLE; - *filling_stage_ = DartFillingStages::INIT; - reset_all_controls(); - - } else if (*switch_left_ == Switch::MIDDLE) { - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - *lifting_left_control_angle = lifting_down_angle_left_; - *lifting_right_control_angle = lifting_down_angle_right_; - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - *lifting_left_control_angle = lifting_down_angle_left_; - *lifting_right_control_angle = lifting_down_angle_right_; + auto sw_left = *switch_left_; + auto sw_right = *switch_right_; + + const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) + && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); + + if (all_down) { + disable_all(); + } else if (sw_left == Switch::MIDDLE) { + // Angle / belt control mode + *yaw_torque_ = std::clamp(joystick_left_->y() * max_torque_, + -max_torque_, max_torque_); + *pitch_torque_ = std::clamp(joystick_right_->x() * max_torque_, + -max_torque_, max_torque_); + + double belt_cmd = joystick_right_->y() * max_belt_torque_; + *belt_left_torque_ = std::clamp(belt_cmd, -max_belt_torque_, max_belt_torque_); + *belt_right_torque_ = std::clamp(belt_cmd, -max_belt_torque_, max_belt_torque_); + + *force_screw_torque_ = 0.0; + + } else if (sw_left == Switch::UP) { + // Force-screw / trigger control mode + *force_screw_torque_ = std::clamp(joystick_right_->x() * max_torque_, + -max_torque_, max_torque_); + *yaw_torque_ = 0.0; + *pitch_torque_ = 0.0; + *belt_left_torque_ = 0.0; + *belt_right_torque_ = 0.0; + + // Right switch transitions: trigger / limiting servo + if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { + *trigger_value_ = trigger_lock_value_; + } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { + *trigger_value_ = trigger_free_value_; } + } - } else if (*switch_left_ == Switch::UP) { - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - *trigger_control_angle = launch_lock_angle_; - *limiting_control_angle = limiting_lock_angle_; - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - *trigger_control_angle = launch_trigger_angle_; - *limiting_control_angle = limiting_trigger_angle_; - } + *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; + log_count_++; + if (log_count_ >= 1000) { + log_count_ = 0; + RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", + *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); } - last_switch_left_ = *switch_left_; - last_switch_right_ = *switch_right_; - last_launch_stage_ = *launch_stage_; - } -private: - void reset_all_controls() { - *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + last_switch_left_ = sw_left; + last_switch_right_ = sw_right; } - void loading_process() { - *limiting_control_angle = limiting_trigger_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; - delay_and_execute(1000, [this]() { - *limiting_control_angle = limiting_lock_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; - }); +private: + void disable_all() { + *trigger_value_ = trigger_lock_value_; + *yaw_torque_ = 0.0; + *pitch_torque_ = 0.0; + *force_screw_torque_ = 0.0; + *belt_left_torque_ = 0.0; + *belt_right_torque_ = 0.0; } rclcpp::TimerBase::SharedPtr timer_; int timer_interval_ms_; std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; + bool is_delaying_ = false; + int delay_remaining_ms_ = 0; void timer_callback() { if (is_delaying_ && delay_remaining_ms_ > 0) { delay_remaining_ms_ -= timer_interval_ms_; if (delay_remaining_ms_ <= 0) { is_delaying_ = false; - if (delayed_action_) { + if (delayed_action_) delayed_action_(); - } } } } - void delay_and_execute(int delay_ms, std::function action) { - if (!is_delaying_) { - is_delaying_ = true; - delay_remaining_ms_ = delay_ms; - delayed_action_ = std::move(action); - } - } + int log_count_ = 0; rclcpp::Logger logger_; + // Remote inputs InputInterface joystick_right_; InputInterface joystick_left_; InputInterface switch_right_; InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_; - rmcs_msgs::Switch last_switch_left_; - - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - - OutputInterface launch_stage_; - rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - - uint16_t launch_lock_angle_; - uint16_t launch_trigger_angle_; - OutputInterface trigger_control_angle; - + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + + // Servo outputs + double trigger_free_value_; + double trigger_lock_value_; + OutputInterface trigger_value_; + + // Motor torque outputs (direct) + double max_torque_; + double max_belt_torque_; + InputInterface force_sensor_ch1_data_; + InputInterface force_sensor_ch2_data_; + OutputInterface yaw_torque_; + OutputInterface pitch_torque_; + OutputInterface force_screw_torque_; + OutputInterface belt_left_torque_; + OutputInterface belt_right_torque_; + + OutputInterface average_force_; + + // Stage output (used by broadcaster) OutputInterface filling_stage_; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_down_angle_right_; - - OutputInterface lifting_left_control_angle; - OutputInterface lifting_right_control_angle; - - uint16_t limiting_lock_angle_; - uint16_t limiting_trigger_angle_; - uint16_t limiting_wait_time_; - - OutputInterface limiting_control_angle; - }; } // namespace rmcs_core::controller::dart #include -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFillingTest, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFillingTest, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp new file mode 100644 index 00000000..4623598c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp @@ -0,0 +1,58 @@ +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::dart { + +// Reads yaw/pitch velocity setpoints from DartManager (/pitch/control/velocity, Eigen::Vector2d, +// [0]=yaw [1]=pitch), applies internal PID controllers, and outputs motor torques directly. +// Force control is fully handled by DartManager + external force_screw_velocity_pid_controller. +class DartSettingController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartSettingController() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , yaw_pid_{ + get_parameter("yaw_kp").as_double(), + get_parameter("yaw_ki").as_double(), + get_parameter("yaw_kd").as_double()} + , pitch_pid_{ + get_parameter("pitch_kp").as_double(), + get_parameter("pitch_ki").as_double(), + get_parameter("pitch_kd").as_double()} { + + register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_); + register_input("/dart/yaw_motor/velocity", yaw_velocity_); + register_input("/dart/pitch_motor/velocity", pitch_velocity_); + + register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); + register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); + } + + void update() override { + const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_; + *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); + *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_); + } + +private: + pid::PidCalculator yaw_pid_; + pid::PidCalculator pitch_pid_; + + InputInterface yaw_pitch_vel_setpoint_; + InputInterface yaw_velocity_; + InputInterface pitch_velocity_; + + OutputInterface yaw_torque_; + OutputInterface pitch_torque_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::dart::DartSettingController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp index 88d50695..0c10e5bd 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp @@ -98,20 +98,23 @@ class DartSettings *force_control_ = joystick_right_->x() * max_transform_rate_ * 5; break; } else if (is_auto_force_control_mode_ == 1) { - *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; transmit_distance_to_force(); force_control(); - log_count_++; - if (log_count_ == 100) { - log_count_ = 0; - RCLCPP_INFO(logger_, "ch1:%d, ch2:%d", *force_sensor_ch1_data_, *force_sensor_ch2_data_); - } - } + } } } while(false); + // Force sensor: always update average and log periodically for verification + *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; + log_count_++; + if (log_count_ >= 100) { + log_count_ = 0; + RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", + *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); + } + // RCLCPP_INFO(this->get_logger(), "yaw_control_speed = %f, pitch_control_speed = %f, force_control_speed = %f", *yaw_control_velocity_, *pitch_control_velocity_, *force_control_); - + }; private: diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp index 3adf136b..f6ba4671 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp @@ -1,325 +1,325 @@ -#include "controller/pid/matrix_pid_calculator.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -launch controls -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -class DartLaunchController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchController() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , timer_interval_ms_(10) - , logger_(get_logger()) - , drive_belt_pid_calculator_( - get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()) { - - drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - - launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); - launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - - limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); - limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); - limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - - register_input("/dart/lifting_left/current_angle", lifting_angle_left_); - register_input("/dart/lifting_right/current_angle", lifting_angle_right_); - - register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); - register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); - - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - register_output("/dart/limiting_servo/control_angle", limiting_control_angle); - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); - - register_output("/dart/filling/stage", filling_stage_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms_), - [this] { timer_callback(); }); - - // *lifting_left_control_angle = lifting_up_angle_left_; - // *lifting_right_control_angle = lifting_up_angle_right_; - } - - void update() override { - using namespace rmcs_msgs; - - if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) - && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { - *launch_stage_ = DartLaunchStages::DISABLE; - *filling_stage_ = DartFillingStages::INIT; - reset_all_controls(); - - } else if (*switch_left_ == Switch::MIDDLE) { - - if (last_launch_stage_ == DartLaunchStages::DISABLE) { - *launch_stage_ = DartLaunchStages::RESETTING; - // *filling_stage_ = DartFillingStages::FILLING; // assume that we already have a dart on the lifting platform at the beginning - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - if (last_launch_stage_ == DartLaunchStages::INIT) { - *launch_stage_ = DartLaunchStages::LOADING; - } else if (last_launch_stage_ == DartLaunchStages::READY) { - *lifting_left_control_angle = lifting_up_angle_left_; - *lifting_right_control_angle = lifting_up_angle_right_; - *launch_stage_ = DartLaunchStages::CANCEL; - } - } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - if (last_launch_stage_ == DartLaunchStages::READY) { - *launch_stage_ = DartLaunchStages::INIT; - trigger_lock_flag_ = false; - delay_and_execute(20, [this]() { - *lifting_left_control_angle = lifting_up_angle_left_; - *lifting_right_control_angle = lifting_up_angle_right_; - *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; - }); - delay_and_execute(500, [this]() { - loading_process(); - }); - } else { - RCLCPP_INFO(logger_, "Dart has't been loaded !"); - } - } - - if (blocking_detection()) { - if (last_launch_stage_ == DartLaunchStages::LOADING) { - *lifting_left_control_angle = lifting_down_angle_left_; - *lifting_right_control_angle = lifting_down_angle_right_; - *filling_stage_ = rmcs_msgs::DartFillingStages::DOWNING; - delay_and_execute(500, [this]() { - *launch_stage_ = DartLaunchStages::RESETTING; - *filling_stage_ = rmcs_msgs::DartFillingStages::READY; - trigger_lock_flag_ = true; - drive_belt_block_count_ = 0; - }); - // if (*lifting_angle_left_ == lifting_down_angle_left_ && *lifting_angle_right_ == lifting_down_angle_right_) { +// #include "controller/pid/matrix_pid_calculator.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// /* +// launch controls +// 键位： +// 双下：全部停止 +// 双中：初始状态 +// 此时{ +// 右拨杆下拨再回中：切换上膛和退膛 +// 处于上膛状态时右拨杆打到上：发射 +// } +// 左拨杆上：设置模式 +// 此时{ +// 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 +// 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 +// } +// */ + +// namespace rmcs_core::controller::dart { +// class DartLaunchController +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// DartLaunchController() +// : Node{ +// get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} +// , timer_interval_ms_(10) +// , logger_(get_logger()) +// , drive_belt_pid_calculator_( +// get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), +// get_parameter("b_kd").as_double()) { + +// drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); +// sync_coefficient_ = get_parameter("sync_coefficient").as_double(); +// max_control_torque_ = get_parameter("max_control_torque").as_double(); + +// launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); +// launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); + +// lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); +// lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); + +// lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); +// lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); + +// limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); +// limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); +// limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); + +// register_input("/remote/joystick/right", joystick_right_); +// register_input("/remote/joystick/left", joystick_left_); +// register_input("/remote/switch/right", switch_right_); +// register_input("/remote/switch/left", switch_left_); + +// register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); +// register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + +// register_input("/dart/lifting_left/current_angle", lifting_angle_left_); +// register_input("/dart/lifting_right/current_angle", lifting_angle_right_); + +// register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); +// register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + +// register_output("/dart/trigger_servo/value", trigger_control_angle); +// register_output("/dart/limiting_servo/control_angle", limiting_control_angle); +// register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); +// register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); + +// register_output("/dart/filling/stage", filling_stage_); + +// timer_ = this->create_wall_timer( +// std::chrono::milliseconds(timer_interval_ms_), +// [this] { timer_callback(); }); + +// // *lifting_left_control_angle = lifting_up_angle_left_; +// // *lifting_right_control_angle = lifting_up_angle_right_; +// } + +// void update() override { +// using namespace rmcs_msgs; + +// if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) +// && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { +// *launch_stage_ = DartLaunchStages::DISABLE; +// *filling_stage_ = DartFillingStages::INIT; +// reset_all_controls(); + +// } else if (*switch_left_ == Switch::MIDDLE) { + +// if (last_launch_stage_ == DartLaunchStages::DISABLE) { +// *launch_stage_ = DartLaunchStages::RESETTING; +// // *filling_stage_ = DartFillingStages::FILLING; // assume that we already have a dart on the lifting platform at the beginning +// } + +// if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { +// if (last_launch_stage_ == DartLaunchStages::INIT) { +// *launch_stage_ = DartLaunchStages::LOADING; +// } else if (last_launch_stage_ == DartLaunchStages::READY) { +// *lifting_left_control_angle = lifting_up_angle_left_; +// *lifting_right_control_angle = lifting_up_angle_right_; +// *launch_stage_ = DartLaunchStages::CANCEL; +// } +// } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { +// if (last_launch_stage_ == DartLaunchStages::READY) { +// *launch_stage_ = DartLaunchStages::INIT; +// trigger_lock_flag_ = false; +// delay_and_execute(20, [this]() { +// *lifting_left_control_angle = lifting_up_angle_left_; +// *lifting_right_control_angle = lifting_up_angle_right_; +// *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; +// }); +// delay_and_execute(500, [this]() { +// loading_process(); +// }); +// } else { +// RCLCPP_INFO(logger_, "Dart has't been loaded !"); +// } +// } + +// if (blocking_detection()) { +// if (last_launch_stage_ == DartLaunchStages::LOADING) { +// *lifting_left_control_angle = lifting_down_angle_left_; +// *lifting_right_control_angle = lifting_down_angle_right_; +// *filling_stage_ = rmcs_msgs::DartFillingStages::DOWNING; +// delay_and_execute(500, [this]() { +// *launch_stage_ = DartLaunchStages::RESETTING; +// *filling_stage_ = rmcs_msgs::DartFillingStages::READY; +// trigger_lock_flag_ = true; +// drive_belt_block_count_ = 0; +// }); +// // if (*lifting_angle_left_ == lifting_down_angle_left_ && *lifting_angle_right_ == lifting_down_angle_right_) { - // *launch_stage_ = DartLaunchStages::RESETTING; - // *filling_stage_ = rmcs_msgs::DartFillingStages::READY; - // trigger_lock_flag_ = true; - // drive_belt_block_count_ = 0; - // } - } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { - *lifting_left_control_angle = lifting_up_angle_left_; - *lifting_right_control_angle = lifting_up_angle_right_; - *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; - delay_and_execute(500, [this]() { - *launch_stage_ = DartLaunchStages::RESETTING; - *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; - trigger_lock_flag_ = false; - drive_belt_block_count_ = 0; - }); - // if (*lifting_angle_left_ == lifting_up_angle_left_ && *lifting_angle_right_ == lifting_up_angle_right_) { - // *launch_stage_ = DartLaunchStages::RESETTING; - // *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; - // trigger_lock_flag_ = false; - // drive_belt_block_count_ = 0; - // }; - } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { - *launch_stage_ = - trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; - *filling_stage_ = - trigger_lock_flag_ ? DartFillingStages::READY : DartFillingStages::INIT; - drive_belt_block_count_ = 0; - } - } - double control_velocity = 0; - - if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { - control_velocity = -drive_belt_working_velocity_; - } else if ( - *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING - || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { - control_velocity = drive_belt_working_velocity_; - } else { - control_velocity = 0; - } - drive_belt_sync_control(control_velocity); - // RCLCPP_INFO(logger_, "%lf", control_velocity); - } - - *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; - - last_switch_left_ = *switch_left_; - last_switch_right_ = *switch_right_; - last_launch_stage_ = *launch_stage_; - } - -private: - void reset_all_controls() { - *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - } - - void drive_belt_sync_control(double set_velocity) { - if (set_velocity == 0) { - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; - - Eigen::Vector2d relative_velocity{ - *left_drive_belt_velocity_ - *right_drive_belt_velocity_, - *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - - Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - - *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); - } - - bool blocking_detection() { - if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) - || (abs(*right_drive_belt_velocity_) < 0.5 - && abs(*right_drive_belt_control_torque_) > 0.5)) { - drive_belt_block_count_++; - } - - return drive_belt_block_count_ > 1000 ? true : false; - } - - void loading_process() { - *limiting_control_angle = limiting_trigger_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; - delay_and_execute(2000, [this]() { - *limiting_control_angle = limiting_lock_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; - }); - } - - rclcpp::TimerBase::SharedPtr timer_; - int timer_interval_ms_; - std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; - - void timer_callback() { - if (is_delaying_ && delay_remaining_ms_ > 0) { - delay_remaining_ms_ -= timer_interval_ms_; - if (delay_remaining_ms_ <= 0) { - is_delaying_ = false; - if (delayed_action_) { - delayed_action_(); - } - } - } - } - - void delay_and_execute(int delay_ms, std::function action) { - if (!is_delaying_) { - is_delaying_ = true; - delay_remaining_ms_ = delay_ms; - delayed_action_ = std::move(action); - } - } - - int drive_belt_block_count_ = 0; - double drive_belt_working_velocity_; - - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_; - rmcs_msgs::Switch last_switch_left_; - - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - OutputInterface left_drive_belt_control_torque_; - OutputInterface right_drive_belt_control_torque_; - InputInterface left_drive_belt_velocity_; - InputInterface right_drive_belt_velocity_; - - double max_control_torque_; - - OutputInterface launch_stage_; - rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - - uint16_t launch_lock_angle_; - uint16_t launch_trigger_angle_; - bool trigger_lock_flag_ = false; - OutputInterface trigger_control_angle; - - OutputInterface filling_stage_; - OutputInterface pulse_sending_flag_; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_down_angle_right_; +// // *launch_stage_ = DartLaunchStages::RESETTING; +// // *filling_stage_ = rmcs_msgs::DartFillingStages::READY; +// // trigger_lock_flag_ = true; +// // drive_belt_block_count_ = 0; +// // } +// } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { +// *lifting_left_control_angle = lifting_up_angle_left_; +// *lifting_right_control_angle = lifting_up_angle_right_; +// *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; +// delay_and_execute(500, [this]() { +// *launch_stage_ = DartLaunchStages::RESETTING; +// *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; +// trigger_lock_flag_ = false; +// drive_belt_block_count_ = 0; +// }); +// // if (*lifting_angle_left_ == lifting_up_angle_left_ && *lifting_angle_right_ == lifting_up_angle_right_) { +// // *launch_stage_ = DartLaunchStages::RESETTING; +// // *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; +// // trigger_lock_flag_ = false; +// // drive_belt_block_count_ = 0; +// // }; +// } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { +// *launch_stage_ = +// trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; +// *filling_stage_ = +// trigger_lock_flag_ ? DartFillingStages::READY : DartFillingStages::INIT; +// drive_belt_block_count_ = 0; +// } +// } +// double control_velocity = 0; + +// if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { +// control_velocity = -drive_belt_working_velocity_; +// } else if ( +// *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING +// || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { +// control_velocity = drive_belt_working_velocity_; +// } else { +// control_velocity = 0; +// } +// drive_belt_sync_control(control_velocity); +// // RCLCPP_INFO(logger_, "%lf", control_velocity); +// } + +// *trigger_control_angle = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; + +// last_switch_left_ = *switch_left_; +// last_switch_right_ = *switch_right_; +// last_launch_stage_ = *launch_stage_; +// } + +// private: +// void reset_all_controls() { +// *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; +// *left_drive_belt_control_torque_ = 0; +// *right_drive_belt_control_torque_ = 0; +// } + +// void drive_belt_sync_control(double set_velocity) { +// if (set_velocity == 0) { +// *left_drive_belt_control_torque_ = 0; +// *right_drive_belt_control_torque_ = 0; +// return; +// } + +// Eigen::Vector2d setpoint_error{ +// set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; + +// Eigen::Vector2d relative_velocity{ +// *left_drive_belt_velocity_ - *right_drive_belt_velocity_, +// *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; + +// Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + +// *left_drive_belt_control_torque_ = +// std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); +// *right_drive_belt_control_torque_ = +// std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); +// } + +// bool blocking_detection() { +// if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) +// || (abs(*right_drive_belt_velocity_) < 0.5 +// && abs(*right_drive_belt_control_torque_) > 0.5)) { +// drive_belt_block_count_++; +// } + +// return drive_belt_block_count_ > 1000 ? true : false; +// } + +// void loading_process() { +// *limiting_control_angle = limiting_trigger_angle_; +// *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; +// delay_and_execute(2000, [this]() { +// *limiting_control_angle = limiting_lock_angle_; +// *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; +// }); +// } + +// rclcpp::TimerBase::SharedPtr timer_; +// int timer_interval_ms_; +// std::function delayed_action_; +// bool is_delaying_ = false; +// int delay_remaining_ms_ = 0; + +// void timer_callback() { +// if (is_delaying_ && delay_remaining_ms_ > 0) { +// delay_remaining_ms_ -= timer_interval_ms_; +// if (delay_remaining_ms_ <= 0) { +// is_delaying_ = false; +// if (delayed_action_) { +// delayed_action_(); +// } +// } +// } +// } + +// void delay_and_execute(int delay_ms, std::function action) { +// if (!is_delaying_) { +// is_delaying_ = true; +// delay_remaining_ms_ = delay_ms; +// delayed_action_ = std::move(action); +// } +// } + +// int drive_belt_block_count_ = 0; +// double drive_belt_working_velocity_; + +// rclcpp::Logger logger_; + +// InputInterface joystick_right_; +// InputInterface joystick_left_; +// InputInterface switch_right_; +// InputInterface switch_left_; +// rmcs_msgs::Switch last_switch_right_; +// rmcs_msgs::Switch last_switch_left_; + +// InputInterface lifting_angle_left_; +// InputInterface lifting_angle_right_; +// OutputInterface left_drive_belt_control_torque_; +// OutputInterface right_drive_belt_control_torque_; +// InputInterface left_drive_belt_velocity_; +// InputInterface right_drive_belt_velocity_; + +// double max_control_torque_; + +// OutputInterface launch_stage_; +// rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + +// double launch_lock_value_; +// double launch_trigger_value_; +// bool trigger_lock_flag_ = false; +// OutputInterface trigger_control_angle; + +// OutputInterface filling_stage_; +// OutputInterface pulse_sending_flag_; + +// uint16_t lifting_up_angle_left_; +// uint16_t lifting_down_angle_left_; +// uint16_t lifting_up_angle_right_; +// uint16_t lifting_down_angle_right_; - OutputInterface lifting_left_control_angle; - OutputInterface lifting_right_control_angle; +// OutputInterface lifting_left_control_angle; +// OutputInterface lifting_right_control_angle; - uint16_t limiting_lock_angle_; - uint16_t limiting_trigger_angle_; - uint16_t limiting_wait_time_; +// uint16_t limiting_lock_angle_; +// uint16_t limiting_trigger_angle_; +// uint16_t limiting_wait_time_; - OutputInterface limiting_control_angle; +// OutputInterface limiting_control_angle; - pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; - double sync_coefficient_; -}; +// pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; +// double sync_coefficient_; +// }; -} // namespace rmcs_core::controller::dart +// } // namespace rmcs_core::controller::dart -#include -#include +// #include +// #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file +// PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp new file mode 100644 index 00000000..a90b1aec --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp @@ -0,0 +1,489 @@ +// #include "controller/pid/matrix_pid_calculator.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// /* +// launch controls +// 键位： +// 双下：全部停止 +// 双中：初始状态 +// 此时{ +// 右拨杆下拨再回中：切换上膛和退膛 +// 处于上膛状态时右拨杆打到上：发射 +// } +// 左拨杆上：设置模式 +// 此时{ +// 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 +// 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 +// } +// */ + +// namespace rmcs_core::controller::dart { +// class DartLaunchControllerV2 +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// DartLaunchControllerV2() +// : Node{ +// get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} +// , timer_interval_ms_(10) +// , logger_(get_logger()) +// , drive_belt_pid_calculator_( +// get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), +// get_parameter("b_kd").as_double()) { + +// drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); +// sync_coefficient_ = get_parameter("sync_coefficient").as_double(); +// max_control_torque_ = get_parameter("max_control_torque").as_double(); + +// trigger_free_ = get_parameter("trigger_free_value").as_double(); +// trigger_lock_ = get_parameter("trigger_lock_value").as_double(); +// register_input("/dart/trigger_flag", trigger_flag_); +// register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); +// register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + +// register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); +// register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); + +// register_output("/dart/trigger_servo/value", trigger_control_angle); + + +// timer_ = this->create_wall_timer( +// std::chrono::milliseconds(timer_interval_ms_), +// [this] { timer_callback(); }); + +// } + +// void update() override { +// using namespace rmcs_msgs; + +// if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) +// && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { +// *launch_stage_ = DartLaunchStages::DISABLE; +// reset_all_controls(); + +// } else if (*switch_left_ == Switch::MIDDLE) { + +// if (last_launch_stage_ == DartLaunchStages::DISABLE) { +// *launch_stage_ = DartLaunchStages::RESETTING; +// } + +// if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { +// if (last_launch_stage_ == DartLaunchStages::INIT) { +// *launch_stage_ = DartLaunchStages::LOADING; +// trigger_lock_flag_ = false; +// } else if (last_launch_stage_ == DartLaunchStages::READY) { +// *launch_stage_ = DartLaunchStages::CANCEL; +// } +// } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { +// if (last_launch_stage_ == DartLaunchStages::READY) { +// *launch_stage_ = DartLaunchStages::INIT; +// trigger_lock_flag_ = false; +// } else { +// RCLCPP_INFO(logger_, "Dart has't been loaded !"); +// } +// } + + +// if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { +// control_velocity = -drive_belt_working_velocity_; +// } else if ( +// *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING +// || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { +// control_velocity = drive_belt_working_velocity_; +// } else { +// control_velocity = 0; +// } +// drive_belt_sync_control(control_velocity); +// } + +// *trigger_control_angle = *trigger_flag_ ? trigger_lock_ : trigger_free_; + +// } + +// private: +// void reset_all_controls() { +// *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; +// *left_drive_belt_control_torque_ = 0; +// *right_drive_belt_control_torque_ = 0; +// } + +// void drive_belt_sync_control(double set_velocity) { +// if (set_velocity == 0) { +// *left_drive_belt_control_torque_ = 0; +// *right_drive_belt_control_torque_ = 0; +// return; +// } + +// Eigen::Vector2d setpoint_error{ +// set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; + +// Eigen::Vector2d relative_velocity{ +// *left_drive_belt_velocity_ - *right_drive_belt_velocity_, +// *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; + +// Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + +// *left_drive_belt_control_torque_ = +// std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); +// *right_drive_belt_control_torque_ = +// std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); +// } + +// rclcpp::TimerBase::SharedPtr timer_; +// int timer_interval_ms_; +// std::function delayed_action_; +// bool is_delaying_ = false; +// int delay_remaining_ms_ = 0; + +// void timer_callback() { +// if (is_delaying_ && delay_remaining_ms_ > 0) { +// delay_remaining_ms_ -= timer_interval_ms_; +// if (delay_remaining_ms_ <= 0) { +// is_delaying_ = false; +// if (delayed_action_) { +// delayed_action_(); +// } +// } +// } +// } + +// void delay_and_execute(int delay_ms, std::function action) { +// if (!is_delaying_) { +// is_delaying_ = true; +// delay_remaining_ms_ = delay_ms; +// delayed_action_ = std::move(action); +// } +// } + +// double control_velocity; +// double drive_belt_working_velocity_; + +// rclcpp::Logger logger_; + +// InputInterface trigger_flag_; +// InputInterface left_drive_belt_velocity_; +// InputInterface right_drive_belt_velocity_; + +// OutputInterface left_drive_belt_control_torque_; +// OutputInterface right_drive_belt_control_torque_; + +// double max_control_torque_; + +// OutputInterface launch_stage_; + +// double trigger_free_; +// double trigger_lock_; +// OutputInterface trigger_control_angle; + +// pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; +// double sync_coefficient_; +// }; + +// } // namespace rmcs_core::controller::dart + +// #include +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchControllerV2, rmcs_executor::Component) + + +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include +// // // #include + +// // // /* +// // // DartLaunchControllerV2 — 发射控制 Phase 1（忽略升降电机与限位舵机） +// // // 仅控制传送带与扳机舵机，不涉及升降平台和限位舵机。 + +// // // 键位： +// // // 双下 / UNKNOWN：DISABLE，停止所有输出 + +// // // 左拨杆中：发射控制模式 +// // // 进入后自动进入 RESETTING（传送带反转，堵转后 → INIT） +// // // INIT + 右拨杆中→下：开始上膛（LOADING，传送带正转，堵转后 → READY） +// // // READY + 右拨杆中→上：发射（trigger FREE，延时后 LOCK，→ RESETTING） +// // // READY + 右拨杆中→下：取消（trigger LOCK，→ RESETTING） + +// // // 左拨杆上：DartSettings 角度/力矩设置模式（launch controller 不干预） +// // // */ + +// // // namespace rmcs_core::controller::dart { + +// // // class DartLaunchControllerV2 +// // // : public rmcs_executor::Component +// // // , public rclcpp::Node { +// // // public: +// // // DartLaunchControllerV2() +// // // : Node{get_component_name(), +// // // rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} +// // // , timer_interval_ms_(10) +// // // , logger_(get_logger()) { + +// // // drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); +// // // sync_coefficient_ = get_parameter("sync_coefficient").as_double(); +// // // max_control_torque_ = get_parameter("max_control_torque").as_double(); +// // // launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); +// // // launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); +// // // trigger_free_duration_ms_ = get_parameter("trigger_free_duration_ms").as_double(); + +// // // // Compatibility outputs: keep servos in safe position (Phase 1 does not actively control) +// // // lifting_up_angle_left_ = static_cast( +// // // get_parameter("lifting_up_angle_left").as_int()); +// // // lifting_up_angle_right_ = static_cast( +// // // get_parameter("lifting_up_angle_right").as_int()); +// // // limiting_lock_angle_ = static_cast( +// // // get_parameter("limiting_lock_angle_").as_int()); + +// // // register_input("/remote/switch/right", switch_right_); +// // // register_input("/remote/switch/left", switch_left_); +// // // register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); +// // // register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + +// // // register_output( +// // // "/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0.0); +// // // register_output( +// // // "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0.0); +// // // register_output("/dart/trigger_servo/value", trigger_value_, launch_lock_value_); + +// // // register_output("/dart/filling/stage", filling_stage_); + +// // // // Required by CatapultDartV3Full hardware (TriggerServo mandatory inputs) +// // // register_output("/dart/limiting_servo/control_angle", limiting_control_angle_, +// // // limiting_lock_angle_); +// // // register_output("/dart/lifting_left/control_angle", lifting_left_control_angle_, +// // // lifting_up_angle_left_); +// // // register_output("/dart/lifting_right/control_angle", lifting_right_control_angle_, +// // // lifting_up_angle_right_); + +// // // timer_ = this->create_wall_timer( +// // // std::chrono::milliseconds(timer_interval_ms_), +// // // [this] { timer_callback(); }); +// // // } + +// // // void update() override { +// // // using namespace rmcs_msgs; + +// // // auto sw_left = *switch_left_; +// // // auto sw_right = *switch_right_; + +// // // const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) +// // // && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); + +// // // if (all_down) { +// // // // SAFETY: stop everything and LOCK trigger immediately. +// // // // reset_all_controls() cancels pending delays and stops belt. +// // // // Trigger is set directly here — never depends on trigger_lock_flag_. +// // // reset_all_controls(); +// // // *trigger_value_ = launch_lock_value_; + +// // // } else if (sw_left == Switch::MIDDLE) { + +// // // // Transition out of DISABLE when entering MIDDLE +// // // if (stage_ == DartLaunchStages::DISABLE) { +// // // stage_ = DartLaunchStages::RESETTING; +// // // drive_belt_block_count_ = 0; +// // // RCLCPP_INFO(logger_, "DISABLE → RESETTING"); +// // // } + +// // // // Switch edge: right MIDDLE → DOWN +// // // if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { +// // // if (stage_ == DartLaunchStages::INIT) { +// // // stage_ = DartLaunchStages::LOADING; +// // // drive_belt_block_count_ = 0; +// // // RCLCPP_INFO(logger_, "INIT → LOADING"); +// // // } else if (stage_ == DartLaunchStages::READY) { +// // // // Cancel without firing. +// // // // Keep trigger_lock_flag_ = true so trigger stays LOCKED +// // // // while belt reverses to reset position (RESETTING → INIT). +// // // stage_ = DartLaunchStages::RESETTING; +// // // drive_belt_block_count_ = 0; +// // // RCLCPP_INFO(logger_, "READY → RESETTING (cancel, trigger stays LOCKED)"); +// // // } + +// // // // Switch edge: right MIDDLE → UP +// // // } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { +// // // if (stage_ == DartLaunchStages::READY) { +// // // // Fire: release trigger, stop belt during fire. +// // // trigger_lock_flag_ = false; +// // // stage_ = DartLaunchStages::INIT; +// // // *filling_stage_ = DartFillingStages::INIT; +// // // RCLCPP_INFO(logger_, "READY → LAUNCH (trigger free for %.0f ms)", +// // // trigger_free_duration_ms_); +// // // delay_and_execute(static_cast(trigger_free_duration_ms_), [this]() { +// // // // Dart has fired: re-lock trigger and start resetting belt. +// // // trigger_lock_flag_ = true; +// // // stage_ = DartLaunchStages::RESETTING; +// // // drive_belt_block_count_ = 0; +// // // RCLCPP_INFO(logger_, "LAUNCH → RESETTING"); +// // // }); +// // // } else { +// // // RCLCPP_WARN(logger_, "Cannot launch: not in READY state (current: %d)", +// // // static_cast(stage_)); +// // // } +// // // } + +// // // // Blocking detection → automatic stage transitions +// // // if (blocking_detection()) { +// // // if (stage_ == DartLaunchStages::RESETTING) { +// // // // Spring/belt fully reset, no dart loaded +// // // stage_ = DartLaunchStages::INIT; +// // // trigger_lock_flag_ = false; +// // // drive_belt_block_count_ = 0; +// // // RCLCPP_INFO(logger_, "RESETTING → INIT (blocked)"); +// // // } else if (stage_ == DartLaunchStages::LOADING) { +// // // // Spring fully compressed, dart loaded +// // // stage_ = DartLaunchStages::READY; +// // // trigger_lock_flag_ = true; +// // // drive_belt_block_count_ = 0; +// // // *filling_stage_ = DartFillingStages::READY; +// // // RCLCPP_INFO(logger_, "LOADING → READY (blocked)"); +// // // } +// // // } + +// // // // Belt velocity based on current stage +// // // double control_velocity = 0.0; +// // // if (stage_ == DartLaunchStages::RESETTING) { +// // // control_velocity = -drive_belt_working_velocity_; +// // // } else if (stage_ == DartLaunchStages::LOADING) { +// // // control_velocity = drive_belt_working_velocity_; +// // // } +// // // drive_belt_sync_control(control_velocity); + +// // // // Trigger: updated only in MIDDLE mode +// // // *trigger_value_ = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; + +// // // } +// // // // LEFT_UP (DartSettings mode): trigger maintains its last written value, no change. + +// // // last_switch_left_ = sw_left; +// // // last_switch_right_ = sw_right; +// // // } + +// // // private: +// // // void reset_all_controls() { +// // // stage_ = rmcs_msgs::DartLaunchStages::DISABLE; +// // // *left_drive_belt_control_torque_ = 0.0; +// // // *right_drive_belt_control_torque_ = 0.0; +// // // // Cancel any pending delayed action (e.g. a mid-launch or post-launch timer) +// // // is_delaying_ = false; +// // // delayed_action_ = nullptr; +// // // trigger_lock_flag_ = false; +// // // drive_belt_block_count_ = 0; +// // // // NOTE: *trigger_value_ is written by the caller (all_down branch) after this returns. +// // // } + +// // // void drive_belt_sync_control(double set_velocity) { +// // // if (set_velocity == 0.0) { +// // // *left_drive_belt_control_torque_ = 0.0; +// // // *right_drive_belt_control_torque_ = 0.0; +// // // return; +// // // } + +// // // Eigen::Vector2d setpoint_error{ +// // // set_velocity - *left_drive_belt_velocity_, +// // // set_velocity - *right_drive_belt_velocity_}; +// // // Eigen::Vector2d relative_velocity{ +// // // *left_drive_belt_velocity_ - *right_drive_belt_velocity_, +// // // *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; +// // // Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + +// // // *left_drive_belt_control_torque_ = +// // // std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); +// // // *right_drive_belt_control_torque_ = +// // // std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); +// // // } + +// // // bool blocking_detection() { +// // // if ((std::abs(*left_drive_belt_velocity_) < 0.5 +// // // && std::abs(*left_drive_belt_control_torque_) > 0.5) +// // // || (std::abs(*right_drive_belt_velocity_) < 0.5 +// // // && std::abs(*right_drive_belt_control_torque_) > 0.5)) { +// // // drive_belt_block_count_++; +// // // } +// // // return drive_belt_block_count_ > 1000; +// // // } + +// // // rclcpp::TimerBase::SharedPtr timer_; +// // // int timer_interval_ms_; +// // // std::function delayed_action_; +// // // bool is_delaying_ = false; +// // // int delay_remaining_ms_ = 0; + +// // // void timer_callback() { +// // // if (is_delaying_ && delay_remaining_ms_ > 0) { +// // // delay_remaining_ms_ -= timer_interval_ms_; +// // // if (delay_remaining_ms_ <= 0) { +// // // is_delaying_ = false; +// // // if (delayed_action_) +// // // delayed_action_(); +// // // } +// // // } +// // // } + +// // // // delay_ms accepts double from yaml (e.g. trigger_free_duration_ms_); cast to int internally. +// // // void delay_and_execute(int delay_ms, std::function action) { +// // // if (!is_delaying_) { +// // // is_delaying_ = true; +// // // delay_remaining_ms_ = delay_ms; +// // // delayed_action_ = std::move(action); +// // // } +// // // } + +// // // rclcpp::Logger logger_; + +// // // InputInterface switch_right_; +// // // InputInterface switch_left_; +// // // rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; +// // // rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + +// // // InputInterface left_drive_belt_velocity_; +// // // InputInterface right_drive_belt_velocity_; +// // // OutputInterface left_drive_belt_control_torque_; +// // // OutputInterface right_drive_belt_control_torque_; + +// // // double drive_belt_working_velocity_; +// // // double sync_coefficient_; +// // // double max_control_torque_; +// // // int drive_belt_block_count_ = 0; + +// // // double launch_trigger_value_; +// // // double launch_lock_value_; +// // // double trigger_free_duration_ms_; +// // // bool trigger_lock_flag_ = false; +// // // OutputInterface trigger_value_; + +// // // rmcs_msgs::DartLaunchStages stage_ = rmcs_msgs::DartLaunchStages::DISABLE; +// // // OutputInterface filling_stage_; + +// // // // Compatibility outputs for CatapultDartV3Full TriggerServo mandatory bindings +// // // uint16_t lifting_up_angle_left_; +// // // uint16_t lifting_up_angle_right_; +// // // uint16_t limiting_lock_angle_; +// // // OutputInterface limiting_control_angle_; +// // // OutputInterface lifting_left_control_angle_; +// // // OutputInterface lifting_right_control_angle_; +// // // }; + +// // // } // namespace rmcs_core::controller::dart + +// // // #include +// // // PLUGINLIB_EXPORT_CLASS( +// // // rmcs_core::controller::dart::DartLaunchControllerV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp new file mode 100644 index 00000000..490c5f9f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp @@ -0,0 +1,359 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* +DartLaunchControllerV2Full — 发射控制 Phase 2（含升降电机与限位舵机） +升降电机：瓴控4005 (LkMotor)，control_angle 接口为 double (rad)。 + yaml 参数用度数，代码内自动换算弧度。 +限位舵机：TriggerServo UART，control_angle 接口为 uint16_t。 + +完整发射流程状态机： + DISABLE → 左中 → RESETTING + RESETTING（带反转）→ 堵转 → INIT（带停，trigger LOCK） + INIT + 右中→下 → LOADING（带正转） + LOADING（带正转）→ 堵转 → 降下升降平台 + 延时500ms + → RESETTING（trigger LOCK）+ loading_process（限位舵机开→延时2s→关） + RESETTING（trigger LOCK）→ 堵转 → READY（带停，trigger LOCK） + READY + 右中→上 → 发射：trigger FREE + 升起升降平台 + → 延时后 trigger LOCK → RESETTING（下一轮） + READY + 右中→下 → CANCEL：升起升降平台 → RESETTING（trigger LOCK=false） +*/ + +namespace rmcs_core::controller::dart { + +class DartLaunchControllerV2Full + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartLaunchControllerV2Full() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , timer_interval_ms_(10) + , logger_(get_logger()) { + + drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); + launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); + trigger_free_duration_ms_ = get_parameter("trigger_free_duration_ms").as_int(); + lifting_settle_ms_ = get_parameter("lifting_settle_ms").as_int(); + limiting_open_duration_ms_ = get_parameter("limiting_open_duration_ms").as_int(); + + // Lifting angles: yaml in degrees, stored as radians + constexpr double kDegToRad = std::numbers::pi / 180.0; + lifting_up_angle_left_ = get_parameter("lifting_up_angle_left_deg").as_double() + * kDegToRad; + lifting_down_angle_left_ = get_parameter("lifting_down_angle_left_deg").as_double() + * kDegToRad; + lifting_up_angle_right_ = get_parameter("lifting_up_angle_right_deg").as_double() + * kDegToRad; + lifting_down_angle_right_ = get_parameter("lifting_down_angle_right_deg").as_double() + * kDegToRad; + + // Limiting servo angles (UART TriggerServo, uint16_t hex values) + limiting_free_angle_ = static_cast( + get_parameter("limiting_free_angle_").as_int()); + limiting_lock_angle_ = static_cast( + get_parameter("limiting_lock_angle_").as_int()); + + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); + + // Lifting angle feedback from LkMotor (double, rad) + register_input("/dart/lifting_left/angle", lifting_angle_left_); + register_input("/dart/lifting_right/angle", lifting_angle_right_); + + register_output( + "/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0.0); + register_output( + "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0.0); + register_output("/dart/trigger_servo/value", trigger_value_, launch_lock_value_); + + // Lifting control: double (rad) → LkMotor control_angle + // Start with NaN to disable motors until explicitly commanded + register_output("/dart/lifting_left/control_angle", lifting_left_control_angle_, + static_cast(std::numeric_limits::quiet_NaN())); + register_output("/dart/lifting_right/control_angle", lifting_right_control_angle_, + static_cast(std::numeric_limits::quiet_NaN())); + + // Limiting servo: uint16_t → TriggerServo control_angle, start locked + register_output("/dart/limiting_servo/control_angle", limiting_control_angle_, + limiting_lock_angle_); + + register_output("/dart/filling/stage", filling_stage_); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms_), + [this] { timer_callback(); }); + } + + void update() override { + using namespace rmcs_msgs; + + auto sw_left = *switch_left_; + auto sw_right = *switch_right_; + + const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) + && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); + + if (all_down) { + reset_all_controls(); + // SAFETY: trigger LOCK immediately, no dependency on flag + *trigger_value_ = launch_lock_value_; + + } else if (sw_left == Switch::MIDDLE) { + + // Transition out of DISABLE + if (stage_ == DartLaunchStages::DISABLE) { + stage_ = DartLaunchStages::RESETTING; + drive_belt_block_count_ = 0; + // Raise platform at startup + *lifting_left_control_angle_ = lifting_up_angle_left_; + *lifting_right_control_angle_ = lifting_up_angle_right_; + *filling_stage_ = DartFillingStages::LIFTING; + RCLCPP_INFO(logger_, "DISABLE → RESETTING, lifting UP"); + } + + // Switch edge: right MIDDLE → DOWN + if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { + if (stage_ == DartLaunchStages::INIT) { + stage_ = DartLaunchStages::LOADING; + drive_belt_block_count_ = 0; + RCLCPP_INFO(logger_, "INIT → LOADING"); + } else if (stage_ == DartLaunchStages::READY) { + // Cancel: lift platform back up, keep trigger LOCKED during CANCEL/RESETTING + *lifting_left_control_angle_ = lifting_up_angle_left_; + *lifting_right_control_angle_ = lifting_up_angle_right_; + *filling_stage_ = DartFillingStages::LIFTING; + stage_ = DartLaunchStages::CANCEL; + drive_belt_block_count_ = 0; + RCLCPP_INFO(logger_, "READY → CANCEL, lifting UP, trigger stays LOCKED"); + } + + // Switch edge: right MIDDLE → UP + } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { + if (stage_ == DartLaunchStages::READY) { + // Launch! + trigger_lock_flag_ = false; + stage_ = DartLaunchStages::INIT; // stop belt during fire + *filling_stage_ = DartFillingStages::INIT; + // Raise platform simultaneously + *lifting_left_control_angle_ = lifting_up_angle_left_; + *lifting_right_control_angle_ = lifting_up_angle_right_; + RCLCPP_INFO(logger_, "READY → LAUNCH (trigger free, lifting UP)"); + delay_and_execute(trigger_free_duration_ms_, [this]() { + trigger_lock_flag_ = true; + stage_ = DartLaunchStages::RESETTING; + drive_belt_block_count_ = 0; + RCLCPP_INFO(logger_, "LAUNCH → RESETTING"); + }); + } else { + RCLCPP_WARN(logger_, "Cannot launch: not in READY state (current: %d)", + static_cast(stage_)); + } + } + + // Blocking detection → automatic stage transitions + if (blocking_detection()) { + if (stage_ == DartLaunchStages::RESETTING) { + // trigger_lock_flag_ determines next state + if (trigger_lock_flag_) { + stage_ = DartLaunchStages::READY; + *filling_stage_ = DartFillingStages::READY; + RCLCPP_INFO(logger_, "RESETTING → READY (blocked, lock=true)"); + } else { + stage_ = DartLaunchStages::INIT; + *filling_stage_ = DartFillingStages::INIT; + RCLCPP_INFO(logger_, "RESETTING → INIT (blocked, lock=false)"); + } + drive_belt_block_count_ = 0; + + } else if (stage_ == DartLaunchStages::LOADING) { + // Lower the lifting platform, then start resetting + *lifting_left_control_angle_ = lifting_down_angle_left_; + *lifting_right_control_angle_ = lifting_down_angle_right_; + *filling_stage_ = DartFillingStages::DOWNING; + drive_belt_block_count_ = 0; + RCLCPP_INFO(logger_, "LOADING blocked → lifting DOWN"); + delay_and_execute(lifting_settle_ms_, [this]() { + stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = true; + drive_belt_block_count_ = 0; + RCLCPP_INFO(logger_, "Lifting settled → RESETTING (lock=true)"); + // Open limiting servo to let dart feed, then close + loading_process(); + }); + + } else if (stage_ == DartLaunchStages::CANCEL) { + // Platform fully raised; RESETTING will decompress spring. + // trigger_lock_flag_ = false so RESETTING → INIT (not READY) + stage_ = DartLaunchStages::RESETTING; + trigger_lock_flag_ = false; + drive_belt_block_count_ = 0; + *filling_stage_ = DartFillingStages::INIT; + RCLCPP_INFO(logger_, "CANCEL → RESETTING (lock=false → INIT)"); + } + } + + // Belt velocity based on current stage + double control_velocity = 0.0; + if (stage_ == DartLaunchStages::RESETTING) { + control_velocity = -drive_belt_working_velocity_; + } else if (stage_ == DartLaunchStages::LOADING + || stage_ == DartLaunchStages::CANCEL) { + control_velocity = drive_belt_working_velocity_; + } + drive_belt_sync_control(control_velocity); + + // Trigger servo: updated only in MIDDLE mode + *trigger_value_ = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; + } + // Note: in LEFT_UP (DartSettings) mode, trigger maintains its last value + + last_switch_left_ = sw_left; + last_switch_right_ = sw_right; + } + +private: + void reset_all_controls() { + stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + *left_drive_belt_control_torque_ = 0.0; + *right_drive_belt_control_torque_ = 0.0; + // Cancel any pending delayed action (e.g. mid-launch or mid-loading timer) + is_delaying_ = false; + delayed_action_ = nullptr; + trigger_lock_flag_ = false; + drive_belt_block_count_ = 0; + *limiting_control_angle_ = limiting_lock_angle_; + // Trigger LOCK is set directly in the all_down branch of update() + } + + void loading_process() { + // Open limiting servo to let dart drop into position + *limiting_control_angle_ = limiting_free_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; + RCLCPP_INFO(logger_, "loading_process: limiting servo OPEN"); + delay_and_execute(limiting_open_duration_ms_, [this]() { + *limiting_control_angle_ = limiting_lock_angle_; + *filling_stage_ = rmcs_msgs::DartFillingStages::READY; + RCLCPP_INFO(logger_, "loading_process: limiting servo LOCK"); + }); + } + + void drive_belt_sync_control(double set_velocity) { + if (set_velocity == 0.0) { + *left_drive_belt_control_torque_ = 0.0; + *right_drive_belt_control_torque_ = 0.0; + return; + } + + Eigen::Vector2d setpoint_error{ + set_velocity - *left_drive_belt_velocity_, + set_velocity - *right_drive_belt_velocity_}; + Eigen::Vector2d relative_velocity{ + *left_drive_belt_velocity_ - *right_drive_belt_velocity_, + *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; + Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; + + *left_drive_belt_control_torque_ = + std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_drive_belt_control_torque_ = + std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + } + + bool blocking_detection() { + if ((std::abs(*left_drive_belt_velocity_) < 0.5 + && std::abs(*left_drive_belt_control_torque_) > 0.5) + || (std::abs(*right_drive_belt_velocity_) < 0.5 + && std::abs(*right_drive_belt_control_torque_) > 0.5)) { + drive_belt_block_count_++; + } + return drive_belt_block_count_ > 1000; + } + + rclcpp::TimerBase::SharedPtr timer_; + int timer_interval_ms_; + std::function delayed_action_; + bool is_delaying_ = false; + int delay_remaining_ms_ = 0; + + void timer_callback() { + if (is_delaying_ && delay_remaining_ms_ > 0) { + delay_remaining_ms_ -= timer_interval_ms_; + if (delay_remaining_ms_ <= 0) { + is_delaying_ = false; + if (delayed_action_) + delayed_action_(); + } + } + } + + void delay_and_execute(int delay_ms, std::function action) { + if (!is_delaying_) { + is_delaying_ = true; + delay_remaining_ms_ = delay_ms; + delayed_action_ = std::move(action); + } + } + + rclcpp::Logger logger_; + + InputInterface switch_right_; + InputInterface switch_left_; + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + + InputInterface left_drive_belt_velocity_; + InputInterface right_drive_belt_velocity_; + OutputInterface left_drive_belt_control_torque_; + OutputInterface right_drive_belt_control_torque_; + + double drive_belt_working_velocity_; + double sync_coefficient_; + double max_control_torque_; + int drive_belt_block_count_ = 0; + + double launch_trigger_value_; + double launch_lock_value_; + int trigger_free_duration_ms_; + bool trigger_lock_flag_ = false; + OutputInterface trigger_value_; + + // Lifting motors (LK4005): double angle in radians + InputInterface lifting_angle_left_; + InputInterface lifting_angle_right_; + OutputInterface lifting_left_control_angle_; + OutputInterface lifting_right_control_angle_; + double lifting_up_angle_left_; + double lifting_down_angle_left_; + double lifting_up_angle_right_; + double lifting_down_angle_right_; + int lifting_settle_ms_; + + // Limiting servo (TriggerServo UART): uint16_t + OutputInterface limiting_control_angle_; + uint16_t limiting_free_angle_; + uint16_t limiting_lock_angle_; + int limiting_open_duration_ms_; + + rmcs_msgs::DartLaunchStages stage_ = rmcs_msgs::DartLaunchStages::DISABLE; + OutputInterface filling_stage_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::dart::DartLaunchControllerV2Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp new file mode 100644 index 00000000..8a9b5f64 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -0,0 +1,116 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::dart { + +// Subscribes to DartManager belt/trigger commands and outputs motor torques + trigger servo value. +// Belt velocity control uses MatrixPidCalculator<2> with sync compensation. +class DartLaunchSetting + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartLaunchSetting() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , belt_pid_{ + get_parameter("b_kp").as_double(), + get_parameter("b_ki").as_double(), + get_parameter("b_kd").as_double()} { + + belt_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + trigger_free_value_ = get_parameter("trigger_free_value").as_double(); + trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); + + register_input("/dart/manager/belt/command", belt_command_); + register_input("/dart/manager/trigger/lock_enable", trigger_lock_enable_); + register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); + + register_output( + "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); + register_output( + "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); + register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); + } + + void update() override { + double control_velocity = 0.0; + switch (*belt_command_) { + case rmcs_msgs::DartSliderStatus::DOWN: + control_velocity = +belt_velocity_; + break; + case rmcs_msgs::DartSliderStatus::UP: + control_velocity = -belt_velocity_; + break; + case rmcs_msgs::DartSliderStatus::WAIT: + default: + control_velocity = 0.0; + break; + } + drive_belt_sync_control(control_velocity); + + *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; + } + +private: + void drive_belt_sync_control(double set_velocity) { + if (set_velocity == 0.0) { + *left_belt_torque_ = 0.0; + *right_belt_torque_ = 0.0; + belt_pid_.reset(); + return; + } + + Eigen::Vector2d setpoint_error{ + set_velocity - *left_belt_velocity_, + set_velocity - *right_belt_velocity_}; + Eigen::Vector2d relative_velocity{ + *left_belt_velocity_ - *right_belt_velocity_, + *right_belt_velocity_ - *left_belt_velocity_}; + + Eigen::Vector2d control_torques = + belt_pid_.update(setpoint_error) - sync_coefficient_ * relative_velocity; + + *left_belt_torque_ = + std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_belt_torque_ = + std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + } + + rclcpp::Logger logger_; + + pid::MatrixPidCalculator<2> belt_pid_; + + double belt_velocity_; + double sync_coefficient_; + double max_control_torque_; + double trigger_free_value_; + double trigger_lock_value_; + + InputInterface belt_command_; + InputInterface trigger_lock_enable_; + InputInterface left_belt_velocity_; + InputInterface right_belt_velocity_; + + OutputInterface left_belt_torque_; + OutputInterface right_belt_torque_; + OutputInterface trigger_value_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::dart::DartLaunchSetting, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 2dfcace0..d8c2d5e8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,25 +1,33 @@ +// #include +// #include + +// #include "hardware/device/can_packet.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/force_sensor.hpp" +// #include "hardware/device/pwm_servo.hpp" +// #include "librmcs/agent/c_board.hpp" +// #include "hardware/device/trigger_servo.hpp" + // #include "filter/low_pass_filter.hpp" // #include "hardware/device/bmi088.hpp" -// #include "hardware/device/dji_motor.hpp" // #include "hardware/device/dr16.hpp" // #include "hardware/device/force_sensor_runtime.hpp" -// #include "hardware/device/trigger_servo.hpp" -// #include "librmcs/agent/c_board.hpp" // #include // #include // #include // #include // #include // #include + // #include // #include -// #include + // #include -// #include -// #include -// #include -// #include -// #include + +// // #include +// // #include +// // #include +// // #include // namespace rmcs_core::hardware { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp index b276a96b..9c6eb2ae 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp @@ -1,142 +1,142 @@ -#include -#include - -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/force_sensor.hpp" -#include "hardware/device/pwm_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartforlibrmcsv3 - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartforlibrmcsv3() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} - , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} - , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} - , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} - , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} - , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} - , force_sensor_{*this} { - - left_belt_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - - right_belt_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - - yaw_control_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - - pitch_control_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - - screw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - } - - CatapultDartforlibrmcsv3(const CatapultDartforlibrmcsv3&) = delete; - CatapultDartforlibrmcsv3& operator=(const CatapultDartforlibrmcsv3&) = delete; - CatapultDartforlibrmcsv3(CatapultDartforlibrmcsv3&&) = delete; - CatapultDartforlibrmcsv3& operator=(CatapultDartforlibrmcsv3&&) = delete; - - ~CatapultDartforlibrmcsv3() override = default; - - void update() override { force_sensor_.update_status(); } - - void command_update() { - auto board = start_transmit(); - board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - - board.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - pitch_control_motor_.generate_command(), - yaw_control_motor_.generate_command(), - screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - board.can2_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - left_belt_motor_.generate_command(), - right_belt_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - } - .as_bytes(), - }); - - if (pub_time_count_++ > 100) { - board.can2_transmit({ - .can_id = 0x301, - .can_data = device::CanPacket8{device::ForceSensor::generate_command()}.as_bytes(), - }); - pub_time_count_ = 0; - } - } - -protected: - void can2_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - - auto can_id = data.can_id; - if (can_id == 0x201) { - pitch_control_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - yaw_control_motor_.store_status(data.can_data); - } else if (can_id == 0x203) { - screw_motor_.store_status(data.can_data); - } else if (can_id == 0x205) { - left_belt_motor_.store_status(data.can_data); - } else if (can_id == 0x206) { - right_belt_motor_.store_status(data.can_data); - } else if (can_id == 0x302) { - force_sensor_.store_status(data.can_data); - } - } - -private: - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartforlibrmcsv3& dart) - : dart_(dart) {} - - void update() override { dart_.command_update(); } - - private: - CatapultDartforlibrmcsv3& dart_; - }; - std::shared_ptr dart_command_; - - rclcpp::Logger logger_; - - device::DjiMotor left_belt_motor_, right_belt_motor_; - device::DjiMotor yaw_control_motor_, pitch_control_motor_; - device::DjiMotor screw_motor_; - device::PWMServo trigger_servo_; - device::ForceSensor force_sensor_; - int pub_time_count_ = 0; -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartforlibrmcsv3, rmcs_executor::Component) \ No newline at end of file +// #include +// #include + +// #include "hardware/device/can_packet.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/force_sensor.hpp" +// #include "hardware/device/pwm_servo.hpp" +// #include "librmcs/agent/c_board.hpp" + +// namespace rmcs_core::hardware { + +// class CatapultDartforlibrmcsv3 +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::agent::CBoard { +// public: +// CatapultDartforlibrmcsv3() +// : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} +// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} +// , dart_command_( +// create_partner_component(get_component_name() + "_command", *this)) +// , logger_{get_logger()} +// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} +// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} +// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} +// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} +// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} +// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} +// , force_sensor_{*this} { + +// left_belt_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + +// right_belt_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(19.)); + +// yaw_control_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + +// pitch_control_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + +// screw_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// } + +// CatapultDartforlibrmcsv3(const CatapultDartforlibrmcsv3&) = delete; +// CatapultDartforlibrmcsv3& operator=(const CatapultDartforlibrmcsv3&) = delete; +// CatapultDartforlibrmcsv3(CatapultDartforlibrmcsv3&&) = delete; +// CatapultDartforlibrmcsv3& operator=(CatapultDartforlibrmcsv3&&) = delete; + +// ~CatapultDartforlibrmcsv3() override = default; + +// void update() override { force_sensor_.update_status(); } + +// void command_update() { +// auto board = start_transmit(); +// board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + +// board.can2_transmit({ +// .can_id = 0x200, +// .can_data = +// device::CanPacket8{ +// pitch_control_motor_.generate_command(), +// yaw_control_motor_.generate_command(), +// screw_motor_.generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// } +// .as_bytes(), +// }); + +// board.can2_transmit({ +// .can_id = 0x1FF, +// .can_data = +// device::CanPacket8{ +// left_belt_motor_.generate_command(), +// right_belt_motor_.generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// } +// .as_bytes(), +// }); + +// if (pub_time_count_++ > 100) { +// board.can2_transmit({ +// .can_id = 0x301, +// .can_data = device::CanPacket8{device::ForceSensor::generate_command()}.as_bytes(), +// }); +// pub_time_count_ = 0; +// } +// } + +// protected: +// void can2_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; + +// auto can_id = data.can_id; +// if (can_id == 0x201) { +// pitch_control_motor_.store_status(data.can_data); +// } else if (can_id == 0x202) { +// yaw_control_motor_.store_status(data.can_data); +// } else if (can_id == 0x203) { +// screw_motor_.store_status(data.can_data); +// } else if (can_id == 0x205) { +// left_belt_motor_.store_status(data.can_data); +// } else if (can_id == 0x206) { +// right_belt_motor_.store_status(data.can_data); +// } else if (can_id == 0x302) { +// force_sensor_.store_status(data.can_data); +// } +// } + +// private: +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDartforlibrmcsv3& dart) +// : dart_(dart) {} + +// void update() override { dart_.command_update(); } + +// private: +// CatapultDartforlibrmcsv3& dart_; +// }; +// std::shared_ptr dart_command_; + +// rclcpp::Logger logger_; + +// device::DjiMotor left_belt_motor_, right_belt_motor_; +// device::DjiMotor yaw_control_motor_, pitch_control_motor_; +// device::DjiMotor screw_motor_; +// device::PWMServo trigger_servo_; +// device::ForceSensor force_sensor_; +// int pub_time_count_ = 0; +// }; +// } // namespace rmcs_core::hardware + +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartforlibrmcsv3, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp new file mode 100644 index 00000000..1419d0a8 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp @@ -0,0 +1,426 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "filter/low_pass_filter.hpp" +#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/force_sensor.hpp" +#include "hardware/device/pwm_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +namespace rmcs_core::hardware { + +class CatapultDartV3Full + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartV3Full() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , pitch_angle_filter_(20.0, 1000.0) + , pitch_velocity_filter_(20.0, 1000.0) + , yaw_velocity_filter_(20.0, 1000.0) + , yaw_filter_(5.0, 1000.0) + , pitch_filter_(10.0, 1000.0) + , roll_filter_(10.0, 1000.0) + , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} + , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} + , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} + , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} + , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} + , force_sensor_{*this} + , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} + + , dr16_{*this} + , imu_{1000, 0.2, 0.0} { + + pitch_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + yaw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + force_screw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_left_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_right_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + + first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); + final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + + setup_imu_coordinate_mapping(); + start_time_ = std::chrono::steady_clock::now(); + yaw_drift_coefficient_ = 0.0; + tf_broadcaster_ = std::make_unique(*this); + + register_output("/tf", tf_); + register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + + force_sensor_calibrate_subscription_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + if (msg->data == 0) { + RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); + auto board = start_transmit(); + board.can1_transmit({ + .can_id = 0x201, + .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), + }); + } else { + RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); + } + } + ); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { + std::lock_guard lock(referee_mutex_); + size_t count = 0; + while (count < size && !referee_ring_buffer_receive_.empty()) { + buffer[count++] = referee_ring_buffer_receive_.front(); + referee_ring_buffer_receive_.pop_front(); + } + return count; + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { + auto board = start_transmit(); + board.uart1_transmit({.uart_data = std::span{buffer, size}}); + return size; + }; + } + + CatapultDartV3Full(const CatapultDartV3Full&) = delete; + CatapultDartV3Full& operator=(const CatapultDartV3Full&) = delete; + CatapultDartV3Full(CatapultDartV3Full&&) = delete; + CatapultDartV3Full& operator=(CatapultDartV3Full&&) = delete; + + ~CatapultDartV3Full() override = default; + + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_left_.update_status(); + drive_belt_motor_right_.update_status(); + force_screw_motor_.update_status(); + force_sensor_.update_status(); + imu_.update_status(); + processImuData(); + } + + void command_update() { + auto board = start_transmit(); + + board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + + if (pub_time_count_++ > 100) { + board.can1_transmit({ + .can_id = 0x301, + .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), + }); + pub_time_count_ = 0; + } + + board.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + pitch_motor_.generate_command(), + yaw_motor_.generate_command(), + force_screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + + }); + + board.can2_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + drive_belt_motor_left_.generate_command(), + drive_belt_motor_right_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }); + } + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + const auto can_id = data.can_id; + + if (can_id == 0x302) { + force_sensor_frame_count_++; + if (force_sensor_frame_count_ % 200 == 1) { + RCLCPP_INFO( + logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", + data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + force_sensor_.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) [[unlikely]] + return; + const auto can_id = data.can_id; + + // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID + if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 + && can_id != 0x205 && can_id != 0x206) { + can2_unknown_count_++; + if (can2_unknown_count_ % 50 == 1) { + RCLCPP_INFO( + logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", + can_id, data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + } + + if (can_id == 0x201) { + pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + force_screw_motor_.store_status(data.can_data); + } else if (can_id == 0x205) { + drive_belt_motor_left_.store_status(data.can_data); + } else if (can_id == 0x206) { + drive_belt_motor_right_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + std::lock_guard lock(referee_mutex_); + for (auto byte : data.uart_data) { + referee_ring_buffer_receive_.push_back(byte); + } + } + + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + } + + 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 { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) + return "[]"; + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + for (size_t i = 0; i < size; ++i) + ss << std::setw(2) << static_cast(data[i]) << " "; + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') + result.pop_back(); + return result; + } + + void setup_imu_coordinate_mapping() { + imu_.set_coordinate_mapping( + [](double x, double y, double z) -> std::tuple { + return {x, -y, -z}; + }); + } + + void processImuData() { + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = + std::chrono::duration(current_time - start_time_).count(); + + Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); + + double roll = std::atan2(rot(2, 1), rot(2, 2)); + double pitch = std::asin(-rot(2, 0)); + double yaw = std::atan2(rot(1, 0), rot(0, 0)); + + double t_roll = -roll_filter_.update(roll); + double t_pitch = pitch_filter_.update(pitch); + double t_yaw = -yaw_filter_.update(yaw); + + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + yaw_samples_.push_back(t_yaw); + time_samples_.push_back(elapsed_seconds); + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + double n = static_cast(yaw_samples_.size()); + for (size_t i = 0; i < yaw_samples_.size(); ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) + yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; + } + + t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + publishTfTransforms(t_roll, t_pitch, t_yaw); + + *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; + *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; + *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; + } + + void publishTfTransforms(double roll, double pitch, double yaw) { + auto now = this->get_clock()->now(); + + auto make_tf = + [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; + + auto make_vec = [](double x, double y, double z) { + geometry_msgs::msg::Vector3 v; + v.x = x; v.y = y; v.z = z; + return v; + }; + + auto make_quat = [](double x, double y, double z, double w) { + geometry_msgs::msg::Quaternion q; + q.x = x; q.y = y; q.z = z; q.w = w; + return q; + }; + + Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; + + auto zero = make_vec(0, 0, 0); + auto p_trans = make_vec(0, 0, 0.05); + auto id_rot = make_quat(0, 0, 0, 1); + + tf_broadcaster_->sendTransform( + make_tf("base_link", "gimbal_center_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf( + "gimbal_center_link", "yaw_link", zero, + make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "yaw_link", "pitch_link", p_trans, + make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "pitch_link", "odom_imu", zero, + make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); + tf_broadcaster_->sendTransform( + make_tf("world", "base_link", zero, id_rot)); + } + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartV3Full& dart) + : dart_(dart) {} + void update() override { dart_.command_update();} + + private: + CatapultDartV3Full& dart_; + }; + + std::shared_ptr dart_command_; + rclcpp::Logger logger_; + + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + filter::LowPassFilter<1> yaw_filter_; + filter::LowPassFilter<1> pitch_filter_; + filter::LowPassFilter<1> roll_filter_; + + std::chrono::steady_clock::time_point start_time_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor force_screw_motor_; + device::DjiMotor drive_belt_motor_left_; + device::DjiMotor drive_belt_motor_right_; + + device::ForceSensor force_sensor_; + + device::PWMServo trigger_servo_; + + device::Dr16 dr16_; + device::Bmi088 imu_; + + std::unique_ptr tf_broadcaster_; + + OutputInterface tf_; + OutputInterface catapult_pitch_angle_; + OutputInterface catapult_roll_angle_; + OutputInterface catapult_yaw_angle_; + rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; + + std::mutex referee_mutex_; + std::deque referee_ring_buffer_receive_; + OutputInterface referee_serial_; + + int pub_time_count_ = 0; + int force_sensor_frame_count_ = 0; + int can2_unknown_count_ = 0; + + double first_sample_spot_ = 1.0; + double final_sample_spot_ = 4.0; + double yaw_drift_coefficient_ = 0.0; + std::vector yaw_samples_; + std::vector time_samples_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp new file mode 100644 index 00000000..6a4ff3ec --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp @@ -0,0 +1,538 @@ +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// #include "filter/low_pass_filter.hpp" +// #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/force_sensor.hpp" +// #include "hardware/device/lk_motor.hpp" +// #include "hardware/device/pwm_servo.hpp" +// #include "hardware/device/trigger_servo.hpp" +// #include "librmcs/agent/c_board.hpp" + +// /* +// CatapultDartV3Lk — catapult_dart_v3_full 的变体 +// 升降电机替换为瓴控4005 (LkMotor MG4005Ei10)，挂 CAN1 (0x141 左, 0x142 右)。 +// 限位舵机保留 TriggerServo (UART2, ID=0x03)。 + +// 升降电机接口 (double, rad): +// 输出: /dart/lifting_left/angle, /dart/lifting_left/velocity 等 +// 输入: /dart/lifting_left/control_angle (由 launch_controller_v2_full 写入) +// */ + +// namespace rmcs_core::hardware { + +// class CatapultDartV3Lk +// : public rmcs_executor::Component +// , public rclcpp::Node +// , private librmcs::agent::CBoard { +// public: +// CatapultDartV3Lk() +// : Node{get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} +// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} +// , dart_command_( +// create_partner_component(get_component_name() + "_command", *this)) +// , logger_{get_logger()} +// , pitch_angle_filter_(20.0, 1000.0) +// , pitch_velocity_filter_(20.0, 1000.0) +// , yaw_velocity_filter_(20.0, 1000.0) +// , yaw_filter_(5.0, 1000.0) +// , pitch_filter_(10.0, 1000.0) +// , roll_filter_(10.0, 1000.0) +// , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} +// , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} +// , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} +// , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} +// , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} +// , force_sensor_{*this} +// , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} +// , limiting_servo_{*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID} +// , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} +// , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} +// , dr16_{*this} +// , imu_{1000, 0.2, 0.0} { + +// pitch_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// yaw_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// force_screw_motor_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// drive_belt_motor_left_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); +// drive_belt_motor_right_.configure( +// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} +// .set_reversed() +// .set_reduction_ratio(19.)); + +// // LK4005 lifting motors: multi-turn angle mode for absolute position control +// lifting_left_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} +// .enable_multi_turn_angle()); +// lifting_right_motor_.configure( +// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} +// .enable_multi_turn_angle()); + +// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); +// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + +// setup_imu_coordinate_mapping(); +// start_time_ = std::chrono::steady_clock::now(); +// yaw_drift_coefficient_ = 0.0; +// tf_broadcaster_ = std::make_unique(*this); + +// register_output("/tf", tf_); +// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); +// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); +// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + +// limiting_calibrate_subscription_ = create_subscription( +// "/limiting/calibrate", rclcpp::QoS{0}, +// [this](std_msgs::msg::Int32::UniquePtr&& msg) { +// trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg)); +// }); + +// register_output("/referee/serial", referee_serial_); +// referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { +// std::lock_guard lock(referee_mutex_); +// size_t count = 0; +// while (count < size && !referee_ring_buffer_receive_.empty()) { +// buffer[count++] = referee_ring_buffer_receive_.front(); +// referee_ring_buffer_receive_.pop_front(); +// } +// return count; +// }; +// referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { +// auto board = start_transmit(); +// board.uart1_transmit({.uart_data = std::span{buffer, size}}); +// return size; +// }; +// } + +// CatapultDartV3Lk(const CatapultDartV3Lk&) = delete; +// CatapultDartV3Lk& operator=(const CatapultDartV3Lk&) = delete; +// CatapultDartV3Lk(CatapultDartV3Lk&&) = delete; +// CatapultDartV3Lk& operator=(CatapultDartV3Lk&&) = delete; +// ~CatapultDartV3Lk() override = default; + +// void update() override { +// dr16_.update_status(); +// pitch_motor_.update_status(); +// yaw_motor_.update_status(); +// drive_belt_motor_left_.update_status(); +// drive_belt_motor_right_.update_status(); +// force_screw_motor_.update_status(); +// force_sensor_.update_status(); +// lifting_left_motor_.update_status(); +// lifting_right_motor_.update_status(); +// imu_.update_status(); +// processImuData(); +// } + +// void command_update() { +// auto board = start_transmit(); + +// // Trigger servo: PWM via GPIO +// board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + +// // Force sensor: polling command on CAN1 (every 100 cycles) +// if (pub_time_count_++ > 100) { +// board.can1_transmit({ +// .can_id = 0x301, +// .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), +// }); +// pub_time_count_ = 0; +// } + +// // DJI motors on CAN2 (pitch/yaw/force_screw: 0x200, belts: 0x1FF) +// board.can2_transmit({ +// .can_id = 0x200, +// .can_data = device::CanPacket8{ +// pitch_motor_.generate_command(), +// yaw_motor_.generate_command(), +// force_screw_motor_.generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// }.as_bytes(), +// }); +// board.can2_transmit({ +// .can_id = 0x1FF, +// .can_data = device::CanPacket8{ +// drive_belt_motor_left_.generate_command(), +// drive_belt_motor_right_.generate_command(), +// device::CanPacket8::PaddingQuarter{}, +// device::CanPacket8::PaddingQuarter{}, +// }.as_bytes(), +// }); + +// // LK4005 lifting motors on CAN1 (unicast per motor) +// board.can1_transmit({ +// .can_id = LK_LIFTING_LEFT_ID, +// .can_data = lifting_left_motor_.generate_command().as_bytes(), +// }); +// board.can1_transmit({ +// .can_id = LK_LIFTING_RIGHT_ID, +// .can_data = lifting_right_motor_.generate_command().as_bytes(), +// }); + +// // Limiting servo on UART2 (only send when target changes) +// if (!limiting_servo_.calibrate_mode()) { +// uint16_t current_target = limiting_servo_.get_target_angle(); +// if (current_target != last_limiting_angle_) { +// size_t uart_data_length; +// auto command_buffer = +// limiting_servo_.generate_runtime_command(uart_data_length); +// board.uart2_transmit( +// {.uart_data = std::span{command_buffer.get(), uart_data_length}}); +// last_limiting_angle_ = current_target; +// } +// } +// } + +// protected: +// void can1_receive_callback(const librmcs::data::CanDataView& data) override { +// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] +// return; +// const auto can_id = data.can_id; + +// if (can_id == 0x302) { +// force_sensor_frame_count_++; +// if (force_sensor_frame_count_ % 200 == 1) { +// RCLCPP_INFO( +// logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s", +// data.can_data.size(), +// bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); +// } +// force_sensor_.store_status(data.can_data); +// } else if (can_id == LK_LIFTING_LEFT_ID) { +// lifting_left_motor_.store_status(data.can_data); +// } else if (can_id == LK_LIFTING_RIGHT_ID) { +// lifting_right_motor_.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) [[unlikely]] +// return; +// const auto can_id = data.can_id; + +// if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 +// && can_id != 0x205 && can_id != 0x206) { +// can2_unknown_count_++; +// if (can2_unknown_count_ % 50 == 1) { +// RCLCPP_INFO( +// logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", +// can_id, data.can_data.size(), +// bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); +// } +// } + +// if (can_id == 0x201) { +// pitch_motor_.store_status(data.can_data); +// } else if (can_id == 0x202) { +// yaw_motor_.store_status(data.can_data); +// } else if (can_id == 0x203) { +// force_screw_motor_.store_status(data.can_data); +// } else if (can_id == 0x205) { +// drive_belt_motor_left_.store_status(data.can_data); +// } else if (can_id == 0x206) { +// drive_belt_motor_right_.store_status(data.can_data); +// } +// } + +// void uart1_receive_callback(const librmcs::data::UartDataView& data) override { +// std::lock_guard lock(referee_mutex_); +// for (auto byte : data.uart_data) { +// referee_ring_buffer_receive_.push_back(byte); +// } +// } + +// void uart2_receive_callback(const librmcs::data::UartDataView& data) override { +// const auto* raw_data = data.uart_data.data(); +// const size_t length = data.uart_data.size(); + +// RCLCPP_DEBUG(logger_, "UART2 received: len=%zu [%s]", length, +// bytes_to_hex_string(raw_data, length).c_str()); + +// if (length < 3) { +// RCLCPP_WARN(logger_, "UART2 data too short: %zu bytes", length); +// return; +// } + +// const uint8_t servo_id = static_cast(raw_data[2]); + +// // Only limiting servo remains on UART2 +// if (servo_id == LIMITING_SERVO_ID) { +// auto result = limiting_servo_.calibrate_current_angle(logger_, raw_data, length); +// if (!result.first) +// RCLCPP_INFO(logger_, "calibrate: uart2 limiting data store failed"); +// } else { +// RCLCPP_DEBUG(logger_, "UART2: unknown servo id 0x%02X", servo_id); +// } +// } + +// 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 { +// imu_.store_accelerometer_status(data.x, data.y, data.z); +// } + +// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { +// imu_.store_gyroscope_status(data.x, data.y, data.z); +// } + +// private: +// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; +// static constexpr uint8_t LIMITING_SERVO_ID = 0x03; +// static constexpr uint32_t LK_LIFTING_LEFT_ID = 0x141; +// static constexpr uint32_t LK_LIFTING_RIGHT_ID = 0x142; + +// void trigger_servo_calibrate_subscription_callback( +// device::TriggerServo& servo, std_msgs::msg::Int32::UniquePtr msg) { +// servo.set_calibrate_mode(msg->data); + +// std::unique_ptr command_buffer; +// size_t command_length = 0; + +// if (msg->data == 0) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); +// } else if (msg->data == 1) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); +// } else if (msg->data == 2) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); +// } else if (msg->data == 3) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); +// } else if (msg->data == 4) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); +// } else if (msg->data == 5) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); +// } else if (msg->data == 6) { +// command_buffer = servo.generate_calibrate_command( +// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); +// } + +// if (command_buffer && command_length > 0) { +// auto board = start_transmit(); +// board.uart2_transmit( +// {.uart_data = std::span{command_buffer.get(), command_length}}); +// RCLCPP_INFO(logger_, "UART2 Pub: (length=%zu)[ %s ]", command_length, +// bytes_to_hex_string(command_buffer.get(), command_length).c_str()); +// } +// } + +// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { +// if (!data || size == 0) +// return "[]"; +// std::stringstream ss; +// ss << std::hex << std::uppercase << std::setfill('0'); +// for (size_t i = 0; i < size; ++i) +// ss << std::setw(2) << static_cast(data[i]) << " "; +// std::string result = ss.str(); +// if (!result.empty() && result.back() == ' ') +// result.pop_back(); +// return result; +// } + +// void setup_imu_coordinate_mapping() { +// imu_.set_coordinate_mapping( +// [](double x, double y, double z) -> std::tuple { +// return {x, -y, -z}; +// }); +// } + +// void processImuData() { +// auto current_time = std::chrono::steady_clock::now(); +// double elapsed_seconds = +// std::chrono::duration(current_time - start_time_).count(); + +// Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); +// Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); + +// double roll = std::atan2(rot(2, 1), rot(2, 2)); +// double pitch = std::asin(-rot(2, 0)); +// double yaw = std::atan2(rot(1, 0), rot(0, 0)); + +// double t_roll = -roll_filter_.update(roll); +// double t_pitch = pitch_filter_.update(pitch); +// double t_yaw = -yaw_filter_.update(yaw); + +// if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { +// yaw_samples_.push_back(t_yaw); +// time_samples_.push_back(elapsed_seconds); + +// double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; +// double n = static_cast(yaw_samples_.size()); +// for (size_t i = 0; i < yaw_samples_.size(); ++i) { +// sum_x += time_samples_[i]; +// sum_y += yaw_samples_[i]; +// sum_xy += time_samples_[i] * yaw_samples_[i]; +// sum_xx += time_samples_[i] * time_samples_[i]; +// } +// double denominator = n * sum_xx - sum_x * sum_x; +// if (std::abs(denominator) > 1e-10) +// yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; +// } + +// t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; +// publishTfTransforms(t_roll, t_pitch, t_yaw); + +// *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; +// *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; +// *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; +// } + +// void publishTfTransforms(double roll, double pitch, double yaw) { +// auto now = this->get_clock()->now(); + +// auto make_tf = +// [&](const std::string& parent, const std::string& child, +// const geometry_msgs::msg::Vector3& trans, +// const geometry_msgs::msg::Quaternion& rot) { +// geometry_msgs::msg::TransformStamped tf; +// tf.header.stamp = now; +// tf.header.frame_id = parent; +// tf.child_frame_id = child; +// tf.transform.translation = trans; +// tf.transform.rotation = rot; +// return tf; +// }; + +// auto make_vec = [](double x, double y, double z) { +// geometry_msgs::msg::Vector3 v; +// v.x = x; v.y = y; v.z = z; +// return v; +// }; + +// auto make_quat = [](double x, double y, double z, double w) { +// geometry_msgs::msg::Quaternion q; +// q.x = x; q.y = y; q.z = z; q.w = w; +// return q; +// }; + +// Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); +// Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); +// Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); +// Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; + +// auto zero = make_vec(0, 0, 0); +// auto p_trans = make_vec(0, 0, 0.05); +// auto id_rot = make_quat(0, 0, 0, 1); + +// tf_broadcaster_->sendTransform( +// make_tf("base_link", "gimbal_center_link", zero, id_rot)); +// tf_broadcaster_->sendTransform(make_tf( +// "gimbal_center_link", "yaw_link", zero, +// make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); +// tf_broadcaster_->sendTransform(make_tf( +// "yaw_link", "pitch_link", p_trans, +// make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); +// tf_broadcaster_->sendTransform(make_tf( +// "pitch_link", "odom_imu", zero, +// make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); +// tf_broadcaster_->sendTransform( +// make_tf("world", "base_link", zero, id_rot)); +// } + +// class DartCommand : public rmcs_executor::Component { +// public: +// explicit DartCommand(CatapultDartV3Lk& dart) +// : dart_(dart) {} +// void update() override { dart_.command_update(); } + +// private: +// CatapultDartV3Lk& dart_; +// }; + +// std::shared_ptr dart_command_; +// rclcpp::Logger logger_; + +// filter::LowPassFilter<1> pitch_angle_filter_; +// filter::LowPassFilter<1> pitch_velocity_filter_; +// filter::LowPassFilter<1> yaw_velocity_filter_; +// filter::LowPassFilter<1> yaw_filter_; +// filter::LowPassFilter<1> pitch_filter_; +// filter::LowPassFilter<1> roll_filter_; + +// std::chrono::steady_clock::time_point start_time_; + +// device::DjiMotor pitch_motor_; +// device::DjiMotor yaw_motor_; +// device::DjiMotor force_screw_motor_; +// device::DjiMotor drive_belt_motor_left_; +// device::DjiMotor drive_belt_motor_right_; + +// device::ForceSensor force_sensor_; + +// device::PWMServo trigger_servo_; +// device::TriggerServo limiting_servo_; + +// // LK4005 lifting motors (replaces TriggerServo lifting servos) +// device::LkMotor lifting_left_motor_; +// device::LkMotor lifting_right_motor_; + +// device::Dr16 dr16_; +// device::Bmi088 imu_; + +// std::unique_ptr tf_broadcaster_; + +// OutputInterface tf_; +// OutputInterface catapult_pitch_angle_; +// OutputInterface catapult_roll_angle_; +// OutputInterface catapult_yaw_angle_; + +// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; + +// std::mutex referee_mutex_; +// std::deque referee_ring_buffer_receive_; +// OutputInterface referee_serial_; + +// int pub_time_count_ = 0; +// int force_sensor_frame_count_ = 0; +// int can2_unknown_count_ = 0; + +// double first_sample_spot_ = 1.0; +// double final_sample_spot_ = 4.0; +// double yaw_drift_coefficient_ = 0.0; +// std::vector yaw_samples_; +// std::vector time_samples_; + +// uint16_t last_limiting_angle_ = 0xFFFF; +// }; + +// } // namespace rmcs_core::hardware + +// #include +// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Lk, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp new file mode 100644 index 00000000..1d981f97 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp @@ -0,0 +1,426 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "filter/low_pass_filter.hpp" +#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/force_sensor.hpp" +#include "hardware/device/pwm_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +namespace rmcs_core::hardware { + +class CatapultDartV4 + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartV4() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , pitch_angle_filter_(20.0, 1000.0) + , pitch_velocity_filter_(20.0, 1000.0) + , yaw_velocity_filter_(20.0, 1000.0) + , yaw_filter_(5.0, 1000.0) + , pitch_filter_(10.0, 1000.0) + , roll_filter_(10.0, 1000.0) + , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} + , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} + , force_screw_motor_{*this, *dart_command_, "/dart/force_screw"} + , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} + , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} + , force_sensor_{*this} + , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} + + , dr16_{*this} + , imu_{1000, 0.2, 0.0} { + + pitch_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + yaw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + force_screw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_left_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_right_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + + first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); + final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + + setup_imu_coordinate_mapping(); + start_time_ = std::chrono::steady_clock::now(); + yaw_drift_coefficient_ = 0.0; + tf_broadcaster_ = std::make_unique(*this); + + register_output("/tf", tf_); + register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + + force_sensor_calibrate_subscription_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + if (msg->data == 0) { + RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); + auto board = start_transmit(); + board.can1_transmit({ + .can_id = 0x201, + .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), + }); + } else { + RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); + } + } + ); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { + std::lock_guard lock(referee_mutex_); + size_t count = 0; + while (count < size && !referee_ring_buffer_receive_.empty()) { + buffer[count++] = referee_ring_buffer_receive_.front(); + referee_ring_buffer_receive_.pop_front(); + } + return count; + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { + auto board = start_transmit(); + board.uart1_transmit({.uart_data = std::span{buffer, size}}); + return size; + }; + } + + CatapultDartV4(const CatapultDartV4&) = delete; + CatapultDartV4& operator=(const CatapultDartV4&) = delete; + CatapultDartV4(CatapultDartV4&&) = delete; + CatapultDartV4& operator=(CatapultDartV4&&) = delete; + + ~CatapultDartV4() override = default; + + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_left_.update_status(); + drive_belt_motor_right_.update_status(); + force_screw_motor_.update_status(); + force_sensor_.update_status(); + imu_.update_status(); + processImuData(); + } + + void command_update() { + auto board = start_transmit(); + + board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + + if (pub_time_count_++ > 100) { + board.can1_transmit({ + .can_id = 0x301, + .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), + }); + pub_time_count_ = 0; + } + + board.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + pitch_motor_.generate_command(), + yaw_motor_.generate_command(), + force_screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + + }); + + board.can2_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + drive_belt_motor_left_.generate_command(), + drive_belt_motor_right_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }); + } + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + const auto can_id = data.can_id; + + if (can_id == 0x302) { + force_sensor_frame_count_++; + if (force_sensor_frame_count_ % 200 == 1) { + RCLCPP_INFO( + logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", + data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + force_sensor_.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) [[unlikely]] + return; + const auto can_id = data.can_id; + + // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID + if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 + && can_id != 0x205 && can_id != 0x206) { + can2_unknown_count_++; + if (can2_unknown_count_ % 50 == 1) { + RCLCPP_INFO( + logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", + can_id, data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + } + + if (can_id == 0x201) { + pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + force_screw_motor_.store_status(data.can_data); + } else if (can_id == 0x205) { + drive_belt_motor_left_.store_status(data.can_data); + } else if (can_id == 0x206) { + drive_belt_motor_right_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + std::lock_guard lock(referee_mutex_); + for (auto byte : data.uart_data) { + referee_ring_buffer_receive_.push_back(byte); + } + } + + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + } + + 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 { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) + return "[]"; + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + for (size_t i = 0; i < size; ++i) + ss << std::setw(2) << static_cast(data[i]) << " "; + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') + result.pop_back(); + return result; + } + + void setup_imu_coordinate_mapping() { + imu_.set_coordinate_mapping( + [](double x, double y, double z) -> std::tuple { + return {x, -y, -z}; + }); + } + + void processImuData() { + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = + std::chrono::duration(current_time - start_time_).count(); + + Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); + + double roll = std::atan2(rot(2, 1), rot(2, 2)); + double pitch = std::asin(-rot(2, 0)); + double yaw = std::atan2(rot(1, 0), rot(0, 0)); + + double t_roll = -roll_filter_.update(roll); + double t_pitch = pitch_filter_.update(pitch); + double t_yaw = -yaw_filter_.update(yaw); + + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + yaw_samples_.push_back(t_yaw); + time_samples_.push_back(elapsed_seconds); + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + double n = static_cast(yaw_samples_.size()); + for (size_t i = 0; i < yaw_samples_.size(); ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) + yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; + } + + t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + publishTfTransforms(t_roll, t_pitch, t_yaw); + + *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; + *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; + *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; + } + + void publishTfTransforms(double roll, double pitch, double yaw) { + auto now = this->get_clock()->now(); + + auto make_tf = + [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; + + auto make_vec = [](double x, double y, double z) { + geometry_msgs::msg::Vector3 v; + v.x = x; v.y = y; v.z = z; + return v; + }; + + auto make_quat = [](double x, double y, double z, double w) { + geometry_msgs::msg::Quaternion q; + q.x = x; q.y = y; q.z = z; q.w = w; + return q; + }; + + Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; + + auto zero = make_vec(0, 0, 0); + auto p_trans = make_vec(0, 0, 0.05); + auto id_rot = make_quat(0, 0, 0, 1); + + tf_broadcaster_->sendTransform( + make_tf("base_link", "gimbal_center_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf( + "gimbal_center_link", "yaw_link", zero, + make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "yaw_link", "pitch_link", p_trans, + make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "pitch_link", "odom_imu", zero, + make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); + tf_broadcaster_->sendTransform( + make_tf("world", "base_link", zero, id_rot)); + } + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartV4& dart) + : dart_(dart) {} + void update() override { dart_.command_update();} + + private: + CatapultDartV4& dart_; + }; + + std::shared_ptr dart_command_; + rclcpp::Logger logger_; + + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + filter::LowPassFilter<1> yaw_filter_; + filter::LowPassFilter<1> pitch_filter_; + filter::LowPassFilter<1> roll_filter_; + + std::chrono::steady_clock::time_point start_time_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor force_screw_motor_; + device::DjiMotor drive_belt_motor_left_; + device::DjiMotor drive_belt_motor_right_; + + device::ForceSensor force_sensor_; + + device::PWMServo trigger_servo_; + + device::Dr16 dr16_; + device::Bmi088 imu_; + + std::unique_ptr tf_broadcaster_; + + OutputInterface tf_; + OutputInterface catapult_pitch_angle_; + OutputInterface catapult_roll_angle_; + OutputInterface catapult_yaw_angle_; + rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; + + std::mutex referee_mutex_; + std::deque referee_ring_buffer_receive_; + OutputInterface referee_serial_; + + int pub_time_count_ = 0; + int force_sensor_frame_count_ = 0; + int can2_unknown_count_ = 0; + + double first_sample_spot_ = 1.0; + double final_sample_spot_ = 4.0; + double yaw_drift_coefficient_ = 0.0; + std::vector yaw_samples_; + std::vector time_samples_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV4, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp index 42e60b0c..3d8d446d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp @@ -1,18 +1,24 @@ -// #include "librmcs/client/cboard.hpp" +// #include +// #include + +// #include "hardware/device/can_packet.hpp" +// #include "hardware/device/dji_motor.hpp" +// #include "hardware/device/force_sensor.hpp" +// #include "hardware/device/pwm_servo.hpp" +// #include "librmcs/agent/c_board.hpp" // #include "hardware/device/dr16.hpp" // #include "hardware/device/trigger_servo.hpp" + // #include // #include // #include // #include // #include -// #include // #include // #include // #include // #include // #include -// #include // #include // #include @@ -21,29 +27,32 @@ // class DartFillingTestHardware // : public rmcs_executor::Component // , public rclcpp::Node -// , private librmcs::client::CBoard { +// , private librmcs::agent::CBoard { // public: // DartFillingTestHardware() // : 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()) -// , dart_command_(create_partner_component(get_component_name() + "_command", -// *this)) , dr16_(*this) , trigger_servo_(*dart_command_, "/dart/trigger_servo", -// TRIGGER_SERVO_ID) , limiting_servo_(*dart_command_, "/dart/limiting_servo", -// LIMITING_SERVO_ID) , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) -// , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { +// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} +// , logger_{get_logger()} +// , dart_command_(create_partner_component(get_component_name() + "_command",*this)) +// , dr16_(*this) +// , limiting_servo_(dart_command_, "/dart/limiting_servo",LIMITING_SERVO_ID) +// , lifting_left_(*this, *this->dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) +// , lifting_right_(*this, *this->dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) +// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} +// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} +// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} +// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} +// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} +// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} +// , force_sensor_{*this} +// // , transmit_buffer_(*this, 32) +// // , event_thread_([this]() { handle_events(); }) +// { // register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); // register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback(trigger_servo_, std::move(msg), -// trigger_servo_uart_data_ptr); -// }); // limiting_calibrate_subscription_ = create_subscription( // "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) // { @@ -64,22 +73,22 @@ // }); // 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_->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; -// }; +// // referee_serial_->write = [this](const std::byte* buffer, size_t size) { +// // transmit_buffer_.add_uart1_transmission(buffer, size); +// // return size; +// // }; // last_read_left_time_ = this->now(); // last_read_right_time_ = this->now(); // } // ~DartFillingTestHardware() override { -// stop_handling_events(); +// // stop_handling_events(); // event_thread_.join(); // } @@ -298,17 +307,19 @@ // std::shared_ptr dart_command_; // device::Dr16 dr16_; -// device::TriggerServo trigger_servo_; // device::TriggerServo limiting_servo_; // device::TriggerServo lifting_left_; // device::TriggerServo lifting_right_; +// device::DjiMotor left_belt_motor_, right_belt_motor_; +// device::DjiMotor yaw_control_motor_, pitch_control_motor_; +// device::DjiMotor screw_motor_; +// device::PWMServo trigger_servo_; +// device::ForceSensor force_sensor_; // rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; // rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; // rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; // rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; -// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; // OutputInterface referee_serial_; -// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; // std::thread event_thread_; // OutputInterface lifting_current_angle_left_; // OutputInterface lifting_current_angle_right_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 89576575..cb795c9f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -1,7 +1,13 @@ #pragma once +#include +#include +#include + +#include +#include + #include -#include #include #include #include @@ -11,76 +17,174 @@ namespace rmcs_core::hardware::device { -class Dr16 : public librmcs::device::Dr16 { +class Dr16 { public: explicit Dr16(rmcs_executor::Component& component) { component.register_output( - "/remote/joystick/right", joystick_right_, Eigen::Vector2d::Zero()); - component.register_output("/remote/joystick/left", joystick_left_, Eigen::Vector2d::Zero()); + "/remote/joystick/right", joystick_right_output_, Eigen::Vector2d::Zero()); + component.register_output( + "/remote/joystick/left", joystick_left_output_, Eigen::Vector2d::Zero()); component.register_output( - "/remote/switch/right", switch_right_, rmcs_msgs::Switch::UNKNOWN); - component.register_output("/remote/switch/left", switch_left_, rmcs_msgs::Switch::UNKNOWN); + "/remote/switch/right", switch_right_output_, rmcs_msgs::Switch::UNKNOWN); + component.register_output( + "/remote/switch/left", switch_left_output_, rmcs_msgs::Switch::UNKNOWN); component.register_output( - "/remote/mouse/velocity", mouse_velocity_, Eigen::Vector2d::Zero()); - component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_); + "/remote/mouse/velocity", mouse_velocity_output_, Eigen::Vector2d::Zero()); + component.register_output("/remote/mouse/mouse_wheel", mouse_wheel_output_); - component.register_output("/remote/mouse", mouse_); - std::memset(&*mouse_, 0, sizeof(*mouse_)); - component.register_output("/remote/keyboard", keyboard_); - std::memset(&*keyboard_, 0, sizeof(*keyboard_)); + component.register_output("/remote/mouse", mouse_output_); + std::memset(&*mouse_output_, 0, sizeof(*mouse_output_)); + component.register_output("/remote/keyboard", keyboard_output_); + std::memset(&*keyboard_output_, 0, sizeof(*keyboard_output_)); - component.register_output("/remote/rotary_knob", rotary_knob_); + component.register_output("/remote/rotary_knob", rotary_knob_output_); // Simulate the rotary knob as a switch, with anti-shake algorithm. component.register_output( - "/remote/rotary_knob_switch", rotary_knob_switch_, rmcs_msgs::Switch::UNKNOWN); + "/remote/rotary_knob_switch", rotary_knob_switch_output_, rmcs_msgs::Switch::UNKNOWN); + } + + void store_status(const std::byte* uart_data, size_t uart_data_length) { + if (uart_data_length != 6 + 8 + 4) + return; + + // Avoid using reinterpret_cast here because it does not account for pointer alignment. + // Dr16DataPart structures are aligned, and using reinterpret_cast on potentially unaligned + // uart_data can cause undefined behavior on architectures that enforce strict alignment + // requirements (e.g., ARM). + // Directly accessing unaligned memory through a casted pointer can lead to crashes, + // inefficiencies, or incorrect data reads. Instead, std::memcpy safely copies the data from + // unaligned memory to properly aligned structures without violating alignment or strict + // aliasing rules. + + uint64_t part1{}; + std::memcpy(&part1, uart_data, 6); + uart_data += 6; + data_part1_.store(part1, std::memory_order::relaxed); + + uint64_t part2{}; + std::memcpy(&part2, uart_data, 8); + uart_data += 8; + data_part2_.store(part2, std::memory_order::relaxed); + + uint32_t part3{}; + std::memcpy(&part3, uart_data, 4); + uart_data += 4; + data_part3_.store(part3, std::memory_order::relaxed); } void update_status() { - librmcs::device::Dr16::update_status(); + auto part1 alignas(uint64_t) = + std::bit_cast(data_part1_.load(std::memory_order::relaxed)); + + auto channel_to_double = [](int32_t value) { + value -= 1024; + if (-660 <= value && value <= 660) + return value / 660.0; + return 0.0; + }; + joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); + joystick_right_.x = channel_to_double(static_cast(part1.joystick_channel1)); + joystick_left_.y = -channel_to_double(static_cast(part1.joystick_channel2)); + joystick_left_.x = channel_to_double(static_cast(part1.joystick_channel3)); + + switch_right_ = static_cast(part1.switch_right); + switch_left_ = static_cast(part1.switch_left); - *joystick_right_ = joystick_right(); - *joystick_left_ = joystick_left(); + auto part2 alignas(uint64_t) = + std::bit_cast(data_part2_.load(std::memory_order::relaxed)); - *switch_right_ = switch_right(); - *switch_left_ = switch_left(); + mouse_velocity_.x = -part2.mouse_velocity_y / 32768.0; + mouse_velocity_.y = -part2.mouse_velocity_x / 32768.0; - *mouse_velocity_ = mouse_velocity(); - *mouse_wheel_ = mouse_wheel(); + mouse_wheel_ = -part2.mouse_velocity_z / 32768.0; - *mouse_ = mouse(); - *keyboard_ = keyboard(); + mouse_.left = part2.mouse_left; + mouse_.right = part2.mouse_right; - *rotary_knob_ = rotary_knob(); + auto part3 alignas(uint32_t) = + std::bit_cast(data_part3_.load(std::memory_order::relaxed)); + + keyboard_ = part3.keyboard; + rotary_knob_ = channel_to_double(part3.rotary_knob); + + *joystick_right_output_ = joystick_right(); + *joystick_left_output_ = joystick_left(); + + *switch_right_output_ = switch_right(); + *switch_left_output_ = switch_left(); + + *mouse_velocity_output_ = mouse_velocity(); + *mouse_wheel_output_ = mouse_wheel(); + + *mouse_output_ = mouse(); + *keyboard_output_ = keyboard(); + + *rotary_knob_output_ = rotary_knob(); update_rotary_knob_switch(); } - Eigen::Vector2d joystick_right() const { - return to_eigen_vector(librmcs::device::Dr16::joystick_right()); - } - Eigen::Vector2d joystick_left() const { - return to_eigen_vector(librmcs::device::Dr16::joystick_left()); - } + struct Vector { + constexpr static Vector zero() { return {.x = 0, .y = 0}; } + double x, y; + }; + + enum class Switch : uint8_t { kUnknown = 0, kUp = 1, kDown = 2, kMiddle = 3 }; + + struct [[gnu::packed]] Mouse { + constexpr static Mouse zero() { + constexpr uint8_t zero = 0; + return std::bit_cast(zero); + } + + bool left : 1; + bool right : 1; + }; + static_assert(sizeof(Mouse) == 1); + + struct [[gnu::packed]] Keyboard { + constexpr static Keyboard zero() { + constexpr uint16_t zero = 0; + return std::bit_cast(zero); + } + + bool w : 1; + bool s : 1; + bool a : 1; + bool d : 1; + bool shift : 1; + bool ctrl : 1; + bool q : 1; + bool e : 1; + bool r : 1; + bool f : 1; + bool g : 1; + bool z : 1; + bool x : 1; + bool c : 1; + bool v : 1; + bool b : 1; + }; + static_assert(sizeof(Keyboard) == 2); + + Eigen::Vector2d joystick_right() const { return to_eigen_vector(joystick_right_); } + Eigen::Vector2d joystick_left() const { return to_eigen_vector(joystick_left_); } rmcs_msgs::Switch switch_right() const { - return std::bit_cast(librmcs::device::Dr16::switch_right()); - } - rmcs_msgs::Switch switch_left() const { - return std::bit_cast(librmcs::device::Dr16::switch_left()); + return std::bit_cast(switch_right_); } + rmcs_msgs::Switch switch_left() const { return std::bit_cast(switch_left_); } - Eigen::Vector2d mouse_velocity() const { - return to_eigen_vector(librmcs::device::Dr16::mouse_velocity()); - } + Eigen::Vector2d mouse_velocity() const { return to_eigen_vector(mouse_velocity_); } - rmcs_msgs::Mouse mouse() const { - return std::bit_cast(librmcs::device::Dr16::mouse()); - } - rmcs_msgs::Keyboard keyboard() const { - return std::bit_cast(librmcs::device::Dr16::keyboard()); - } + rmcs_msgs::Mouse mouse() const { return std::bit_cast(mouse_); } + rmcs_msgs::Keyboard keyboard() const { return std::bit_cast(keyboard_); } + + double rotary_knob() const { return rotary_knob_; } + + double mouse_wheel() const { return mouse_wheel_; } private: static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } @@ -89,7 +193,7 @@ class Dr16 : public librmcs::device::Dr16 { constexpr double divider = 0.7, anti_shake_shift = 0.05; double upper_divider = divider, lower_divider = -divider; - auto& switch_value = *rotary_knob_switch_; + auto& switch_value = *rotary_knob_switch_output_; if (switch_value == rmcs_msgs::Switch::UP) upper_divider -= anti_shake_shift, lower_divider -= anti_shake_shift; else if (switch_value == rmcs_msgs::Switch::MIDDLE) @@ -97,7 +201,7 @@ class Dr16 : public librmcs::device::Dr16 { else if (switch_value == rmcs_msgs::Switch::DOWN) upper_divider += anti_shake_shift, lower_divider += anti_shake_shift; - const auto knob_value = -*rotary_knob_; + const auto knob_value = -*rotary_knob_output_; if (knob_value > upper_divider) { switch_value = rmcs_msgs::Switch::UP; } else if (knob_value < lower_divider) { @@ -107,20 +211,86 @@ class Dr16 : public librmcs::device::Dr16 { } } - rmcs_executor::Component::OutputInterface joystick_right_; - rmcs_executor::Component::OutputInterface joystick_left_; + struct [[gnu::packed]] Dr16DataPart1 { + uint64_t joystick_channel0 : 11; + uint64_t joystick_channel1 : 11; + uint64_t joystick_channel2 : 11; + uint64_t joystick_channel3 : 11; + + uint64_t switch_right : 2; + uint64_t switch_left : 2; + + uint64_t padding : 16; + }; + static_assert(sizeof(Dr16DataPart1) == 8); + std::atomic data_part1_{std::bit_cast(Dr16DataPart1{ + .joystick_channel0 = 1024, + .joystick_channel1 = 1024, + .joystick_channel2 = 1024, + .joystick_channel3 = 1024, + .switch_right = static_cast(Switch::kDown), + .switch_left = static_cast(Switch::kDown), + .padding = 0, + })}; + static_assert(decltype(data_part1_)::is_always_lock_free); + + struct [[gnu::packed]] Dr16DataPart2 { + int16_t mouse_velocity_x; + int16_t mouse_velocity_y; + int16_t mouse_velocity_z; + + bool mouse_left; + bool mouse_right; + }; + static_assert(sizeof(Dr16DataPart2) == 8); + std::atomic data_part2_{std::bit_cast(Dr16DataPart2{ + .mouse_velocity_x = 0, + .mouse_velocity_y = 0, + .mouse_velocity_z = 0, + .mouse_left = false, + .mouse_right = false, + })}; + static_assert(decltype(data_part2_)::is_always_lock_free); + + struct [[gnu::packed]] Dr16DataPart3 { + Keyboard keyboard; + uint16_t rotary_knob; + }; + static_assert(sizeof(Dr16DataPart3) == 4); + std::atomic data_part3_ = {std::bit_cast(Dr16DataPart3{ + .keyboard = Keyboard::zero(), + .rotary_knob = 0, + })}; + static_assert(decltype(data_part3_)::is_always_lock_free); + + Vector joystick_right_ = Vector::zero(); + Vector joystick_left_ = Vector::zero(); + + Switch switch_right_ = Switch::kUnknown; + Switch switch_left_ = Switch::kUnknown; + + Vector mouse_velocity_ = Vector::zero(); + + Mouse mouse_ = Mouse::zero(); + Keyboard keyboard_ = Keyboard::zero(); + + double rotary_knob_ = 0.0; + double mouse_wheel_ = 0.0; + + rmcs_executor::Component::OutputInterface joystick_right_output_; + rmcs_executor::Component::OutputInterface joystick_left_output_; - rmcs_executor::Component::OutputInterface switch_right_; - rmcs_executor::Component::OutputInterface switch_left_; + rmcs_executor::Component::OutputInterface switch_right_output_; + rmcs_executor::Component::OutputInterface switch_left_output_; - rmcs_executor::Component::OutputInterface mouse_velocity_; - rmcs_executor::Component::OutputInterface mouse_wheel_; + rmcs_executor::Component::OutputInterface mouse_velocity_output_; + rmcs_executor::Component::OutputInterface mouse_wheel_output_; - rmcs_executor::Component::OutputInterface mouse_; - rmcs_executor::Component::OutputInterface keyboard_; + rmcs_executor::Component::OutputInterface mouse_output_; + rmcs_executor::Component::OutputInterface keyboard_output_; - rmcs_executor::Component::OutputInterface rotary_knob_; - rmcs_executor::Component::OutputInterface rotary_knob_switch_; + rmcs_executor::Component::OutputInterface rotary_knob_output_; + rmcs_executor::Component::OutputInterface rotary_knob_switch_output_; }; } // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp index c6a1b769..9a4e991d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -5,22 +5,24 @@ #include #include #include +#include #include #include -// #include namespace rmcs_core::hardware::device { class ForceSensor { public: explicit ForceSensor(rmcs_executor::Component& status_component) { - status_component.register_output("/force_sensor/channel_1/weight", weight_ch1_, nan_); status_component.register_output("/force_sensor/channel_2/weight", weight_ch2_, nan_); } void store_status(std::span can_data) { - can_data_.store(CanPacket8{can_data}, std::memory_order_relaxed); + ForceSensorStatus status{}; + std::memcpy(&status, can_data.data(), + std::min(can_data.size(), sizeof(ForceSensorStatus))); + force_sensor_status_.store(status, std::memory_order_relaxed); } void update_status() { @@ -28,33 +30,29 @@ class ForceSensor { uint32_t ch1_weight = static_cast(status.ch1_0) << 24 | static_cast(status.ch1_1) << 16 - | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); + | static_cast(status.ch1_2) << 8 | static_cast(status.ch1_3); uint32_t ch2_weight = static_cast(status.ch2_0) << 24 | static_cast(status.ch2_1) << 16 - | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); + | static_cast(status.ch2_2) << 8 | static_cast(status.ch2_3); *weight_ch1_ = std::bit_cast(ch1_weight); *weight_ch2_ = std::bit_cast(ch2_weight); } - static uint64_t generate_command() { return 0x00; } + // static uint64_t generate_command() { return 0x00; } + static uint64_t generate_zero_calibration_command() { return 0x0F; } + + + static CanPacket8::Quarter generate_command(){return CanPacket8::Quarter{0};} private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); struct __attribute__((packed, aligned(8))) ForceSensorStatus { - uint8_t ch1_0; - uint8_t ch1_1; - uint8_t ch1_2; - uint8_t ch1_3; - uint8_t ch2_0; - uint8_t ch2_1; - uint8_t ch2_2; - uint8_t ch2_3; + uint8_t ch1_0, ch1_1, ch1_2, ch1_3; + uint8_t ch2_0, ch2_1, ch2_2, ch2_3; }; std::atomic force_sensor_status_{}; - - std::atomic can_data_; static_assert(decltype(force_sensor_status_)::is_always_lock_free); rmcs_executor::Component::OutputInterface weight_ch1_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp deleted file mode 100644 index c9fc7e7f..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/hojo_board.hpp +++ /dev/null @@ -1,96 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class HojoBoard { -public: - HojoBoard(rmcs_executor::Component& command_component, - const std::string& name, - uint8_t id_first, uint8_t id_second) - : id_first_(id_first), id_second_(id_second) { - command_component.register_input(name + "_first/control_angle", control_angle_first_); - command_component.register_input(name + "_second/control_angle", control_angle_second_); - command_component.register_input(name + "_first/runtime", runtime_first_); - command_component.register_input(name + "_second/runtime", runtime_second_); - - } - - std::unique_ptr generate_sync_run_command(size_t& output_length) { - size_t size = sizeof(WrightSyncControlAngleCommand); - std::unique_ptr buffer = std::make_unique(size); - WrightSyncControlAngleCommand command; - command.header[0] = 0xFF; - command.header[1] = 0xFF; - command.id = 0xFE; - command.data_length = 0x0E; - command.command_type = 0x83; - command.command_address = 0x2A; - command.command_address_length = 0x04; - command.cowork_id_one = id_first_; - command.control_angle_one[0] = static_cast(*control_angle_first_ >> 8); - command.control_angle_one[1] = static_cast(*control_angle_first_ & 0x00FF); - command.runtime_one[0] = static_cast(*runtime_first_ >> 8); - command.runtime_one[1] = static_cast(*runtime_first_ & 0x00FF); - command.cowork_id_two = id_second_; - command.control_angle_two[0] = static_cast(*control_angle_second_ >> 8); - command.control_angle_two[1] = static_cast(*control_angle_second_ & 0x00FF); - command.runtime_two[0] = static_cast(*runtime_second_ >> 8); - command.runtime_two[1] = static_cast(*runtime_second_ & 0x00FF); - command.checksum = command.calculate_checksum(); - - std::memcpy(buffer.get(), &command, size); - output_length = size; - return buffer; - } - -private: - struct __attribute__((packed)) WrightSyncControlAngleCommand { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t command_type; - uint8_t command_address; - uint8_t command_address_length; - uint8_t cowork_id_one; - uint8_t control_angle_one[2]; - uint8_t runtime_one[2]; - uint8_t cowork_id_two; - uint8_t control_angle_two[2]; - uint8_t runtime_two[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + command_type + command_address + command_address_length - + cowork_id_one + control_angle_one[0] + control_angle_one[1] + runtime_one[0] + runtime_one[1] - + cowork_id_two + control_angle_two[0] + control_angle_two[1] + runtime_two[0] + runtime_two[1]; - return ~check; - } - }; - - template - std::unique_ptr copy_to_buffer(const T& cmd, size_t& out_len) { - auto buffer = std::make_unique(sizeof(T)); - std::memcpy(buffer.get(), &cmd, sizeof(T)); - out_len = sizeof(T); - return buffer; - } - - uint8_t id_first_; - uint8_t id_second_; - - rmcs_executor::Component::InputInterface control_angle_first_; - rmcs_executor::Component::InputInterface control_angle_second_; - rmcs_executor::Component::InputInterface runtime_first_; - rmcs_executor::Component::InputInterface runtime_second_; -}; - -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp deleted file mode 100644 index 2fa9d356..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_left.hpp +++ /dev/null @@ -1,175 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class LiftingLeft { -public: - LiftingLeft(rmcs_executor::Component& command_component, const std::string& name) { - command_component.register_input(name + "/control_angle", control_angle_); - } - - enum class CalibrateOperation { - SWITCH_TO_SERVO_MODE, - SWITCH_TO_MOTOR_MODE, - MOTOR_FORWARD_MODE, - MOTOR_REVERSE_MODE, - MOTOR_RUNTIME_CONTROL, - MOTOR_DISABLE_CONTROL, - READ_CURRENT_ANGLE, - }; - - bool calibrate_mode() const { return is_calibrate_mode_; } - - void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } - - std::unique_ptr generate_runtime_command(size_t& output_length) { - size_t size = sizeof(WrightControlAngleCommand); - std::unique_ptr buffer = std::make_unique(size); - WrightControlAngleCommand command; - command.header[0] = 0xFF; - command.header[1] = 0xFF; - command.id = 0x03; - command.data_length = 0x07; - command.command_type = 0x03; - command.command_address = 0x2A; - command.control_angle[0] = static_cast(*control_angle_ >> 8); - command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); - command.control_runtime[0] = 0x00; - command.control_runtime[1] = 0x00; - command.checksum = command.calculate_checksum(); - - std::memcpy(buffer.get(), &command, size); - output_length = size; - - return buffer; - } - - std::unique_ptr - generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { - uint8_t* command = nullptr; - size_t command_size = 0; - - if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { - command_size = sizeof(switch_servo_mode_); - command = switch_servo_mode_; - } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { - command_size = sizeof(switch_motor_mode_); - command = switch_motor_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { - command_size = sizeof(switch_motor_forward_mode_); - command = switch_motor_forward_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { - command_size = sizeof(switch_motor_reverse_mode_); - command = switch_motor_reverse_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { - command_size = sizeof(motor_mode_runtime_control_); - command = motor_mode_runtime_control_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { - command_size = sizeof(motor_mode_disable_control_); - command = motor_mode_disable_control_; - } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { - command_size = sizeof(read_angle_command_); - command = read_angle_command_; - } - - std::unique_ptr buffer = std::make_unique(command_size); - std::memcpy(buffer.get(), command, command_size); - output_length = command_size; - - return buffer; - } - - bool calibrate_current_angle( - const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != sizeof(ReadAngleReceiveData)) { - RCLCPP_INFO(logger, "UART data length not match"); - return false; - } - - ReadAngleReceiveData package; - uint8_t checksum; - - std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); - std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); - - if (package.header[0] != 0xFF || package.header[1] != 0xF5) { - RCLCPP_INFO(logger, "UART data header not match"); - return false; - } - - if (checksum != package.calculate_checksum()) { - RCLCPP_INFO( - logger, "Checksum error: receive:%x,calc:%x", checksum, - package.calculate_checksum()); - return false; - } - - uint16_t current_angle = static_cast(package.current_angle[0]) << 8 - | static_cast(package.current_angle[1]); - - current_angle_ = current_angle; - - RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); - return true; - } - - uint16_t get_current_angle() const { - return current_angle_.load(std::memory_order_acquire); - } - -private: - struct __attribute__((packed)) WrightControlAngleCommand { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t command_type; - uint8_t command_address; - uint8_t control_angle[2]; - uint8_t control_runtime[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + command_type + command_address + control_angle[0] - + control_angle[1] + control_runtime[0] + control_runtime[1]; - return ~check; - } - }; - - struct __attribute__((packed)) ReadAngleReceiveData { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t servo_status; - uint8_t current_angle[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; - return ~check; - } - }; - uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x02, 0x38, 0x02, 0xBC}; - - uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1C, 0x01, 0xD8}; - uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1C, 0x00, 0xD9}; - - uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1D, 0x00, 0xD8}; - uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x03, 0x04, 0x03, 0x1D, 0x01, 0xD7}; - - uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x03, 0x05, 0x03, 0x41, 0x00, 0x14, 0x9F}; - uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x03, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB3}; - rmcs_executor::Component::InputInterface control_angle_; // 0-4095 - - std::atomic is_calibrate_mode_ = false; - std::atomic current_angle_{0}; -}; -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp deleted file mode 100644 index f027f83c..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lifting_right.hpp +++ /dev/null @@ -1,176 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class LiftingRight { -public: - LiftingRight(rmcs_executor::Component& command_component, const std::string& name) { - command_component.register_input(name + "/control_angle", control_angle_); - } - - enum class CalibrateOperation { - SWITCH_TO_SERVO_MODE, - SWITCH_TO_MOTOR_MODE, - MOTOR_FORWARD_MODE, - MOTOR_REVERSE_MODE, - MOTOR_RUNTIME_CONTROL, - MOTOR_DISABLE_CONTROL, - READ_CURRENT_ANGLE, - }; - - bool calibrate_mode() const { return is_calibrate_mode_; } - - void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } - - std::unique_ptr generate_runtime_command(size_t& output_length) { - size_t size = sizeof(WrightControlAngleCommand); - std::unique_ptr buffer = std::make_unique(size); - WrightControlAngleCommand command; - command.header[0] = 0xFF; - command.header[1] = 0xFF; - command.id = 0x04; - command.data_length = 0x07; - command.command_type = 0x03; - command.command_address = 0x2A; - command.control_angle[0] = static_cast(*control_angle_ >> 8); - command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); - command.control_runtime[0] = 0x00; - command.control_runtime[1] = 0x00; - command.checksum = command.calculate_checksum(); - - std::memcpy(buffer.get(), &command, size); - output_length = size; - - return buffer; - } - - std::unique_ptr - generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { - uint8_t* command = nullptr; - size_t command_size = 0; - - if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { - command_size = sizeof(switch_servo_mode_); - command = switch_servo_mode_; - } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { - command_size = sizeof(switch_motor_mode_); - command = switch_motor_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { - command_size = sizeof(switch_motor_forward_mode_); - command = switch_motor_forward_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { - command_size = sizeof(switch_motor_reverse_mode_); - command = switch_motor_reverse_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { - command_size = sizeof(motor_mode_runtime_control_); - command = motor_mode_runtime_control_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { - command_size = sizeof(motor_mode_disable_control_); - command = motor_mode_disable_control_; - } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { - command_size = sizeof(read_angle_command_); - command = read_angle_command_; - } - - std::unique_ptr buffer = std::make_unique(command_size); - std::memcpy(buffer.get(), command, command_size); - output_length = command_size; - - return buffer; - } - - bool calibrate_current_angle( - const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != sizeof(ReadAngleReceiveData)) { - RCLCPP_INFO(logger, "UART data length not match"); - return false; - } - - ReadAngleReceiveData package; - uint8_t checksum; - - std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); - std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); - - if (package.header[0] != 0xFF || package.header[1] != 0xF5) { - RCLCPP_INFO(logger, "UART data header not match"); - return false; - } - - if (checksum != package.calculate_checksum()) { - RCLCPP_INFO( - logger, "Checksum error: receive:%x,calc:%x", checksum, - package.calculate_checksum()); - return false; - } - - uint16_t current_angle = static_cast(package.current_angle[0]) << 8 - | static_cast(package.current_angle[1]); - - current_angle_.store(current_angle, std::memory_order_release); - - RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); - return true; - } - - uint16_t get_current_angle() const { - return current_angle_.load(std::memory_order_acquire); - } - -private: - struct __attribute__((packed)) WrightControlAngleCommand { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t command_type; - uint8_t command_address; - uint8_t control_angle[2]; - uint8_t control_runtime[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + command_type + command_address + control_angle[0] - + control_angle[1] + control_runtime[0] + control_runtime[1]; - return ~check; - } - }; - - struct __attribute__((packed)) ReadAngleReceiveData { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t servo_status; - uint8_t current_angle[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; - return ~check; - } - }; - uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x02, 0x38, 0x02, 0xBB}; - - uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1C, 0x01, 0xD7}; - uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1C, 0x00, 0xD8}; - - uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1D, 0x00, 0xD7}; - uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x04, 0x04, 0x03, 0x1D, 0x01, 0xD6}; - - uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x04, 0x05, 0x03, 0x41, 0x00, 0x14, 0x9E}; - uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x04, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB2}; - - rmcs_executor::Component::InputInterface control_angle_; // 0-4095 - - std::atomic is_calibrate_mode_ = false; - std::atomic current_angle_{0}; -}; -} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp deleted file mode 100644 index 6b8c4b64..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/limiting_servo.hpp +++ /dev/null @@ -1,622 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::hardware::device { - -class LimitingServo { -public: - LimitingServo(rmcs_executor::Component& command_component, const std::string& name) { - command_component.register_input(name + "/control_angle", control_angle_); - } - - enum class CalibrateOperation { - SWITCH_TO_SERVO_MODE, - SWITCH_TO_MOTOR_MODE, - MOTOR_FORWARD_MODE, - MOTOR_REVERSE_MODE, - MOTOR_RUNTIME_CONTROL, - MOTOR_DISABLE_CONTROL, - READ_CURRENT_ANGLE, - }; - - bool calibrate_mode() const { return is_calibrate_mode_; } - - void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } - - std::unique_ptr generate_runtime_command(size_t& output_length) { - size_t size = sizeof(WrightControlAngleCommand); - std::unique_ptr buffer = std::make_unique(size); - WrightControlAngleCommand command; - command.header[0] = 0xFF; - command.header[1] = 0xFF; - command.id = 0x02; - command.data_length = 0x07; - command.command_type = 0x03; - command.command_address = 0x2A; - command.control_angle[0] = static_cast(*control_angle_ >> 8); - command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); - command.control_runtime[0] = 0x00; - command.control_runtime[1] = 0x00; - command.checksum = command.calculate_checksum(); - - std::memcpy(buffer.get(), &command, size); - output_length = size; - - return buffer; - } - - std::unique_ptr - generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { - uint8_t* command = nullptr; - size_t command_size = 0; - - if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { - command_size = sizeof(switch_servo_mode_); - command = switch_servo_mode_; - } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { - command_size = sizeof(switch_motor_mode_); - command = switch_motor_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { - command_size = sizeof(switch_motor_forward_mode_); - command = switch_motor_forward_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { - command_size = sizeof(switch_motor_reverse_mode_); - command = switch_motor_reverse_mode_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { - command_size = sizeof(motor_mode_runtime_control_); - command = motor_mode_runtime_control_; - } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { - command_size = sizeof(motor_mode_disable_control_); - command = motor_mode_disable_control_; - } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { - command_size = sizeof(read_angle_command_); - command = read_angle_command_; - } - - std::unique_ptr buffer = std::make_unique(command_size); - std::memcpy(buffer.get(), command, command_size); - output_length = command_size; - - return buffer; - } - - static bool calibrate_current_angle( - const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { - if (uart_data_length != sizeof(ReadAngleReceiveData)) { - RCLCPP_INFO(logger, "UART data length not match"); - return false; - } - - ReadAngleReceiveData package; - uint8_t checksum; - - std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); - std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); - - if (package.header[0] != 0xFF || package.header[1] != 0xF5) { - RCLCPP_INFO(logger, "UART data header not match"); - return false; - } - - if (checksum != package.calculate_checksum()) { - RCLCPP_INFO( - logger, "Checksum error: receive:%x,calc:%x", checksum, - package.calculate_checksum()); - return false; - } - - uint16_t current_angle = static_cast(package.current_angle[0]) << 8 - | static_cast(package.current_angle[1]); - RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle); - return true; - } - -private: - struct __attribute__((packed)) WrightControlAngleCommand { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t command_type; - uint8_t command_address; - uint8_t control_angle[2]; - uint8_t control_runtime[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + command_type + command_address + control_angle[0] - + control_angle[1] + control_runtime[0] + control_runtime[1]; - return ~check; - } - }; - - struct __attribute__((packed)) ReadAngleReceiveData { - uint8_t header[2]; - uint8_t id; - uint8_t data_length; - uint8_t servo_status; - uint8_t current_angle[2]; - uint8_t checksum; - - uint8_t calculate_checksum() const { - uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; - return ~check; - } - }; - uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x02, 0x38, 0x02, 0xBD}; - - uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1C, 0x01, 0xD9}; - uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1C, 0x00, 0xDA}; - - uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1D, 0x00, 0xD9}; - uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x02, 0x04, 0x03, 0x1D, 0x01, 0xD8}; - - uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x02, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA0}; - uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x02, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB4}; - - rmcs_executor::Component::InputInterface control_angle_; // 0-4095 - - std::atomic is_calibrate_mode_ = false; -}; -} - -// namespace rmcs_core::hardware::device -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware::device { - -// enum class CalibrateOperation { -// SWITCH_TO_SERVO_MODE, -// SWITCH_TO_MOTOR_MODE, -// MOTOR_FORWARD_MODE, -// MOTOR_REVERSE_MODE, -// MOTOR_RUNTIME_CONTROL, -// MOTOR_DISABLE_CONTROL, -// READ_CURRENT_ANGLE, -// }; - -// class TriggerServo { -// public: -// TriggerServo(rmcs_executor::Component& command_component, const std::string& name, uint8_t device_id_ = 0x01) { -// command_component.register_input(name + "/control_angle", control_angle_, device_id_); -// } -// bool calibrate_mode() const { return is_calibrate_mode_; } - -// void set_calibrate_mode(bool mode) { is_calibrate_mode_ = mode; } - -// std::unique_ptr generate_runtime_command(size_t& output_length) { -// size_t size = sizeof(WrightControlAngleCommand); -// std::unique_ptr buffer = std::make_unique(size); -// WrightControlAngleCommand command; -// command.header[0] = 0xFF; -// command.header[1] = 0xFF; -// command.id = device_id_; -// command.data_length = 0x07; -// command.command_type = 0x03; -// command.command_address = 0x2A; -// command.control_angle[0] = static_cast(*control_angle_ >> 8); -// command.control_angle[1] = static_cast(*control_angle_ & 0x00FF); -// command.control_runtime[0] = 0x00; -// command.control_runtime[1] = 0x00; -// command.checksum = command.calculate_checksum(); - -// std::memcpy(buffer.get(), &command, size); -// output_length = size; - -// // RCLCPP_INFO( -// // logger, "%x, %x, %x, %x", *control_angle_, *control_angle_ >> 8, static_cast(*control_angle_ >> -// // 8), static_cast(*control_angle_ & 0x00FF)); -// return buffer; -// } - -// std::unique_ptr -// generate_calibrate_command(CalibrateOperation calibrate_operation, size_t& output_length) { -// uint8_t* command = nullptr; -// size_t command_size = 0; - -// if (calibrate_operation == CalibrateOperation::SWITCH_TO_SERVO_MODE) { -// command_size = sizeof(switch_servo_mode_); -// command = switch_servo_mode_; -// } else if (calibrate_operation == CalibrateOperation::SWITCH_TO_MOTOR_MODE) { -// command_size = sizeof(switch_motor_mode_); -// command = switch_motor_mode_; -// } else if (calibrate_operation == CalibrateOperation::MOTOR_FORWARD_MODE) { -// command_size = sizeof(switch_motor_forward_mode_); -// command = switch_motor_forward_mode_; -// } else if (calibrate_operation == CalibrateOperation::MOTOR_REVERSE_MODE) { -// command_size = sizeof(switch_motor_reverse_mode_); -// command = switch_motor_reverse_mode_; -// } else if (calibrate_operation == CalibrateOperation::MOTOR_RUNTIME_CONTROL) { -// command_size = sizeof(motor_mode_runtime_control_); -// command = motor_mode_runtime_control_; -// } else if (calibrate_operation == CalibrateOperation::MOTOR_DISABLE_CONTROL) { -// command_size = sizeof(motor_mode_disable_control_); -// command = motor_mode_disable_control_; -// } else if (calibrate_operation == CalibrateOperation::READ_CURRENT_ANGLE) { -// command_size = sizeof(read_angle_command_); -// command = read_angle_command_; -// } - -// std::unique_ptr buffer = std::make_unique(command_size); -// std::memcpy(buffer.get(), command, command_size); -// output_length = command_size; - -// return buffer; -// } - -// bool -// calibrate_current_angle(const rclcpp::Logger& logger, const std::byte* uart_data, size_t uart_data_length) { -// if (uart_data_length != sizeof(ReadAngleReceiveData)) { -// RCLCPP_INFO(logger, "UART data length not match"); -// return false; -// } - -// ReadAngleReceiveData package; -// uint8_t checksum; - -// std::memcpy(&package, uart_data, sizeof(ReadAngleReceiveData)); -// std::memcpy(&checksum, uart_data + sizeof(ReadAngleReceiveData) - 1, sizeof(uint8_t)); - -// if (package.header[0] != 0xFF || package.header[1] != 0xF5) { -// RCLCPP_INFO(logger, "UART data header not match"); -// return false; -// } - -// if (checksum != package.calculate_checksum()) { -// RCLCPP_INFO(logger, "Checksum error: receive:%x,calc:%x", checksum, package.calculate_checksum()); -// return false; -// } - -// current_angle_ = -// static_cast(package.current_angle[0]) << 8 | static_cast(package.current_angle[1]); -// RCLCPP_INFO(logger, "Servo Current Angle: 0x%04X", current_angle_); -// return true; -// } - -// uint16_t get_current_angle_() const { return current_angle_; } - -// private: -// uint8_t device_id_; -// uint16_t current_angle_; - -// struct __attribute__((packed)) WrightControlAngleCommand { -// uint8_t header[2]; -// uint8_t id; -// uint8_t data_length; -// uint8_t command_type; -// uint8_t command_address; -// uint8_t control_angle[2]; -// uint8_t control_runtime[2]; -// uint8_t checksum; - -// uint8_t calculate_checksum() const { -// uint8_t check = id + data_length + command_type + command_address + control_angle[0] + control_angle[1] -// + control_runtime[0] + control_runtime[1]; -// return ~check; -// } -// }; - -// struct __attribute__((packed)) ReadAngleReceiveData { -// uint8_t header[2]; -// uint8_t id; -// uint8_t data_length; -// uint8_t servo_status; -// uint8_t current_angle[2]; -// uint8_t checksum; - -// uint8_t calculate_checksum() const { -// uint8_t check = id + data_length + servo_status + current_angle[0] + current_angle[1]; -// return ~check; -// } -// }; -// uint8_t read_angle_command_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x02, 0x38, 0x02, 0xBE}; - -// uint8_t switch_servo_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x01, 0xDA}; -// uint8_t switch_motor_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1C, 0x00, 0xDB}; - -// uint8_t switch_motor_forward_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x00, 0xDA}; -// uint8_t switch_motor_reverse_mode_[8] = {0xFF, 0xFF, 0x01, 0x04, 0x03, 0x1D, 0x01, 0xD9}; - -// uint8_t motor_mode_runtime_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x14, 0xA1}; -// uint8_t motor_mode_disable_control_[9] = {0xFF, 0xFF, 0x01, 0x05, 0x03, 0x41, 0x00, 0x00, 0xB5}; - -// rmcs_executor::Component::InputInterface control_angle_; // 0-4095 - -// std::atomic is_calibrate_mode_ = false; -// }; -// } -// namespace rmcs_core::hardware::device -// // #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/trigger_servo.hpp" -// #include "librmcs/client/cboard.hpp" -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware { -// class CatapultDart -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::client::CBoard { -// public: -// CatapultDart() -// : 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()) -// , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , dr16_(*this) -// , pitch_motor_( -// *this, *dart_command_, "/dart/pitch_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , yaw_motor_( -// *this, *dart_command_, "/dart/yaw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , force_control_motor_( -// *this, *dart_command_, "/dart/force_screw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , drive_belt_motor_( -// {*this, *dart_command_, "/dart/drive_belt/left", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, -// {*this, *dart_command_, "/dart/drive_belt/right", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508} -// .set_reduction_ratio(19.) -// .set_reversed()}) -// , force_sensor_(*this) -// , trigger_servo_(*dart_command_, "/dart/trigger_servo") -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { - -// force_sensor_calibrate_subscription_ = create_subscription( -// "/force_sensor/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// force_sensor_calibrate_subscription_callback(std::move(msg)); -// }); - -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_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; -// }; -// } - -// ~CatapultDart() override { -// stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_[0].update_status(); -// drive_belt_motor_[1].update_status(); -// force_control_motor_.update_status(); -// force_sensor_.update_status(); -// } - -// void command_update() { -// uint16_t can_commands[4]; - -// if (force_sensor_pub_time_count_++ > 100) { -// force_sensor_pub_time_count_ = 0; - -// transmit_buffer_.add_can1_transmission( -// force_sensor_.get_can_id(device::ForceSensor::Mode::MEASUREMENT, 1), -// force_sensor_.generate_read_weight_command()); -// } - -// can_commands[0] = pitch_motor_.generate_command(); -// can_commands[1] = yaw_motor_.generate_command(); -// can_commands[2] = force_control_motor_.generate_command(); -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - -// can_commands[0] = drive_belt_motor_[0].generate_command(); -// can_commands[1] = drive_belt_motor_[1].generate_command(); -// can_commands[2] = 0; -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - -// if (!trigger_servo_.calibrate_mode()) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, uart_data_length); -// } - -// transmit_buffer_.trigger_transmission(); -// } - -// 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 == 0x302) { -// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] -// return; - -// if (can_id == 0x201) { -// pitch_motor_.store_status(can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(can_data); -// } else if (can_id == 0x203) { -// force_control_motor_.store_status(can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_[0].store_status(can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_[1].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 uart2_receive_callback(const std::byte* data, uint8_t length) override { -// // bool success = trigger_servo_.calibrate_current_angle(logger_, data, length); -// // if (!success) { -// // RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// // } - -// // // std::string hex_string = bytes_to_hex_string(data, length); -// // // RCLCPP_INFO(this->get_logger(), "UART2(length: %hhu): [ %s ]", length, -// // // hex_string.c_str()); -// // } - -// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { -// dr16_.store_status(uart_data, uart_data_length); -// } - -// private: -// void force_sensor_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { -// transmit_buffer_.add_can1_transmission( -// force_sensor_.get_can_id(device::ForceSensor::Mode::ZERO_CALIBRATE, 1), -// force_sensor_.generate_zero_calibration_command()); -// RCLCPP_INFO(logger_, "call"); -// } - -// void trigger_servo_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { -// /* -// 标定命令格式： -// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" -// 替换data值就行 -// */ -// trigger_servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = trigger_servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, command_length); - -// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); -// RCLCPP_INFO( -// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, -// hex_string.c_str()); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } // DEBUG TOOL - -// rclcpp::Logger logger_; - -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDart& robot) -// : dart_(robot) {} - -// void update() override { dart_.command_update(); } - -// CatapultDart& dart_; -// }; -// std::shared_ptr dart_command_; - -// device::Dr16 dr16_; - -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_control_motor_; -// device::DjiMotor drive_belt_motor_[2]; - -// device::ForceSensor force_sensor_; -// int force_sensor_pub_time_count_ = 0; // ForceSensor仅支持10Hz - -// device::TriggerServo trigger_servo_; - -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; - -// 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::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index e00dea1c..9f0a7be5 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit e00dea1c0ddc9ccadb239398f82d647afbd0d5be +Subproject commit 9f0a7be53926d2451ccc7c70567bf294699fec38 diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/component.cpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/component.cpp new file mode 100644 index 00000000..561ac7a2 --- /dev/null +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/component.cpp @@ -0,0 +1,7 @@ +#include "rmcs_executor/component.hpp" + +namespace rmcs_executor { + +const char* Component::initializing_component_name; + +} // namespace rmcs_executor \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp new file mode 100644 index 00000000..a818d50a --- /dev/null +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/executor.hpp @@ -0,0 +1,185 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "predefined_msg_provider.hpp" +#include "rmcs_executor/component.hpp" + +namespace rmcs_executor { + +class Executor final : public rclcpp::Node { +public: + explicit Executor( + const std::string& node_name, rclcpp::executors::SingleThreadedExecutor& rcl_executor) + : Node{node_name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)} + , rcl_executor_(rcl_executor) { + Component::initializing_component_name = "predefined_msg_provider"; + predefined_msg_provider_ = std::make_shared(); + add_component(predefined_msg_provider_); + } + ~Executor() { + if (thread_.joinable()) + thread_.join(); + }; + + void add_component(std::shared_ptr component) { + component_list_.emplace_back(component); + if (auto node = std::dynamic_pointer_cast(component)) + rcl_executor_.add_node(node); + for (auto& partner_component : component->partner_component_list_) + add_component(partner_component); + } + + void start() { + init(); + + for (auto& component : component_list_) + component->before_updating(); + + double update_rate; + if (!get_parameter("update_rate", update_rate)) + throw std::runtime_error{"Unable to get parameter update_rate"}; + predefined_msg_provider_->set_update_rate(update_rate); + + thread_ = std::thread{[update_rate, this]() { + const auto period = std::chrono::nanoseconds( + static_cast(std::round(1'000'000'000.0 / update_rate))); + auto next_iteration_time = std::chrono::steady_clock::now(); + while (rclcpp::ok()) { + predefined_msg_provider_->set_timestamp(next_iteration_time); + next_iteration_time += period; + for (const auto& component : updating_order_) { + component->update(); + } + std::this_thread::sleep_until(next_iteration_time); + } + }}; + } + +private: + void init() { + updating_order_.clear(); + + auto output_map = std::unordered_map{}; + auto user_output_map = std::map{}; + for (const auto& component : component_list_) { + component->dependency_count_ = 0; + component->wanted_by_.clear(); + for (auto& output : component->output_list_) { + if (!output_map.emplace(output.name, &output).second) + throw std::runtime_error{"Duplicate names of output"}; + user_output_map.emplace(output.name, output.type); + } + } + + for (auto& component : component_list_) { + component->before_pairing(user_output_map); + } + + for (const auto& component : component_list_) { + for (const auto& input : component->input_list_) { + auto output_iter = output_map.find(input.name); + if (output_iter == output_map.end()) { + if (!input.required) + continue; + + RCLCPP_FATAL( + get_logger(), + "Cannot find the corresponding output of input \"%s\" declared by " + "component [%s]", + input.name.c_str(), component->get_component_name().c_str()); + throw std::runtime_error{"Cannot find the corresponding output"}; + } + + const auto& output = *output_iter->second; + if (input.type != output.type) { + RCLCPP_FATAL(get_logger(), "With message \"%s\":", input.name.c_str()); + RCLCPP_FATAL( + get_logger(), " Component [%s] declared the output with type \"%s\"", + output.component->get_component_name().c_str(), output.type.name()); + RCLCPP_FATAL( + get_logger(), " Component [%s] requested the input with type \"%s\"", + component->get_component_name().c_str(), input.type.name()); + RCLCPP_FATAL(get_logger(), "Type not match."); + throw std::runtime_error{"Type not match"}; + } + + if (output.component->wanted_by_.emplace(component.get()).second) + component->dependency_count_++; + + *input.pointer_to_data_pointer = output.data_pointer; + } + } + + RCLCPP_INFO(get_logger(), "Calculating component dependencies"); + std::vector independent_list; + for (const auto& component : component_list_) { + if (component->dependency_count_ == 0) + independent_list.emplace_back(component.get()); + } + for (const auto& component : independent_list) { + append_updating_order(component); + } + + if (updating_order_.size() < component_list_.size()) { + RCLCPP_FATAL(get_logger(), "Circular dependency found:"); + for (const auto& component : component_list_) { + if (component->dependency_count_ == 0) + continue; + + RCLCPP_FATAL( + get_logger(), "Component [%s]:", component->get_component_name().c_str()); + for (const auto& input : component->input_list_) { + const auto& output = output_map[input.name]; + if (output->component->dependency_count_ == 0) + continue; + RCLCPP_FATAL( + get_logger(), " Depends on [%s] because requesting \"%s\"", + output->component->component_name_.c_str(), output->name.c_str()); + } + } + throw std::runtime_error{"Circular dependency found"}; + } + } + + void append_updating_order(Component* updatable_component) { + std::string space = "- "; + for (size_t i = dependency_recursive_level_; i-- > 0;) + space.append(" "); + RCLCPP_INFO( + get_logger(), "%s%s", space.c_str(), updatable_component->get_component_name().c_str()); + updating_order_.emplace_back(updatable_component); + + for (auto& component : component_list_) { + if (updatable_component->wanted_by_.contains(component.get())) { + if (--component->dependency_count_ == 0) { + dependency_recursive_level_++; + append_updating_order(component.get()); + dependency_recursive_level_--; + } + } + } + }; + + rclcpp::executors::SingleThreadedExecutor& rcl_executor_; + + std::thread thread_; + + std::shared_ptr predefined_msg_provider_; + std::vector> component_list_; + + std::vector updating_order_; + size_t dependency_recursive_level_ = 0; +}; + +}; // namespace rmcs_executor \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp new file mode 100644 index 00000000..99311f24 --- /dev/null +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/main.cpp @@ -0,0 +1,73 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "executor.hpp" +#include "rmcs_executor/component.hpp" + +void segmentation_fault_handler(int) { + void* array[100]; + + int size = backtrace(array, 100); + fprintf(stderr, "[Fatal] Segmentation fault\n>>> STACK TRACE BEGIN\n"); + + // Print the stack trace to stderr. + if (size >= 2) + // Remove the stack trace used to call this function. + backtrace_symbols_fd(array + 2, size - 2, STDERR_FILENO); + else + backtrace_symbols_fd(array, size, STDERR_FILENO); + + fprintf(stderr, "<<< STACK TRACE END\n"); + + exit(1); +} + +int main(int argc, char** argv) { + std::signal(SIGSEGV, segmentation_fault_handler); + + rclcpp::init(argc, argv); + + pluginlib::ClassLoader component_loader( + "rmcs_executor", "rmcs_executor::Component"); + + rclcpp::executors::SingleThreadedExecutor rcl_executor; + auto executor = std::make_shared("rmcs_executor", rcl_executor); + rcl_executor.add_node(executor); + + std::vector component_descriptions; + if (!executor->get_parameter("components", component_descriptions)) + throw std::runtime_error("Missing parameter 'components' or config is not found"); + + std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); + for (const auto& component_description : component_descriptions) { + std::smatch matches; + std::string plugin_name, component_name; + + if (std::regex_search(component_description, matches, regex)) { + if (matches.size() != 3) + throw std::runtime_error("In regex matching: unexpected number of matches"); + + plugin_name = matches[1].str(); + component_name = matches[2].str(); + } else { + plugin_name = component_name = component_description; + } + + rmcs_executor::Component::initializing_component_name = component_name.c_str(); + auto component = component_loader.createSharedInstance(plugin_name); + executor->add_component(component); + } + + executor->start(); + rcl_executor.spin(); + + rclcpp::shutdown(); +} \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp new file mode 100644 index 00000000..68b1716e --- /dev/null +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/src/predefined_msg_provider.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "rmcs_executor/component.hpp" +#include + +class PredefinedMsgProvider : public rmcs_executor::Component { +public: + PredefinedMsgProvider() { + register_output("/predefined/update_rate", update_rate_); + register_output("/predefined/update_count", update_count_, static_cast(-1)); + register_output("/predefined/timestamp", timestamp_); + } + + void set_update_rate(double frame_rate) { *update_rate_ = frame_rate; } + void set_timestamp(std::chrono::steady_clock::time_point timestamp) { *timestamp_ = timestamp; } + + void update() override { *update_count_ += 1; } + +private: + OutputInterface update_rate_; + OutputInterface update_count_; + OutputInterface timestamp_; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_slider_status.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_slider_status.hpp new file mode 100644 index 00000000..e7af19db --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_slider_status.hpp @@ -0,0 +1,11 @@ +#pragma once + +namespace rmcs_msgs { + +enum class DartSliderStatus { + UP = 0, + DOWN = 1, + WAIT = 2, +}; + +} // namespace rmcs_msgs \ No newline at end of file From 766dd094fb414350400e92b65832ea0208b518b9 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 15 Mar 2026 22:40:52 +0800 Subject: [PATCH 37/45] develop 1to4shots dealing --- .../config/dart-filling-test.yaml | 2 +- .../rmcs_bringup/config/dart-guidance-v2.yaml | 71 ++ .../rmcs_bringup/config/dart-guidance.yaml | 4 +- .../src/rmcs_bringup/config/dart-launch.yaml | 68 -- .../rmcs_bringup/config/dart-pitch-pid.yaml | 98 -- .../config/dart-remot-bridge-test.yaml | 16 + rmcs_ws/src/rmcs_bringup/config/dart.yaml | 91 -- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../src/controller/dart/dart_filling_test.cpp | 21 +- .../src/controller/dart/dart_setting.cpp | 1 + .../src/controller/dart/dart_settings.cpp | 185 --- .../src/controller/dart/launch_controller.cpp | 325 ----- .../controller/dart/launch_controller_v2.cpp | 489 -------- .../src/controller/dart/launch_setting.cpp | 13 +- .../src/controller/dart/launch_setting_v2.cpp | 152 +++ .../src/hardware/catapult_dart_v3_lk.cpp | 1074 ++++++++--------- .../src/hardware/catapult_dart_v4.cpp | 23 + rmcs_ws/src/rmcs_dart_guidance | 2 +- 18 files changed, 830 insertions(+), 1808 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml create mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml index 88c9baee..17f481b7 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml @@ -2,7 +2,7 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::CatapultDartV3Full -> catapult_dart + - rmcs_core::hardware::CatapultDartV3Lk -> catapult_dart - rmcs_core::controller::dart::DartFillingTest -> launch_controller catapult_dart: diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml new file mode 100644 index 00000000..6f654128 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml @@ -0,0 +1,71 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware + - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager + - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge + - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting + - rmcs_core::controller::dart::DartSettingController -> dart_setting + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + +dart_hardware: + ros__parameters: + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 + +dart_manager: + ros__parameters: + max_transform_rate: 15.0 + manual_force_scale: 5.0 + limiting_open_angle: 500 # uint16_t，需实测标定 + limiting_close_angle: 1000 # uint16_t，需实测标定 + limiting_fill_ticks: 500 + lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 + lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz + lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 + lifting_stall_timeout_ticks: 5000 # 超时帧数 = 5s + +remote_cmd_bridge: + ros__parameters: + joystick_sensitivity: 0.01 + +launch_setting: + ros__parameters: + belt_velocity: 15.0 + sync_coefficient: 0.1 + max_control_torque: 15.0 + trigger_free_value: 0.75 + trigger_lock_value: 0.63 + b_kp: 5.0 + b_ki: 0.0 + b_kd: 0.0 + lifting_velocity: 10.0 # rad/s，正值 = 左+右-（平台上升方向），需实测标定方向 + belt_hold_torque: 15.0 # Nm，传送带下方堵转后的保持扭矩，需实测标定 + +dart_setting: + ros__parameters: + yaw_kp: 1.0 + yaw_ki: 0.0 + yaw_kd: 0.0 + pitch_kp: 1.0 + pitch_ki: 0.0 + pitch_kd: 0.0 + +force_screw_velocity_pid_controller: + ros__parameters: + measurement: /dart/force_screw_motor/velocity + setpoint: /force/control/velocity + control: /dart/force_screw_motor/control_torque + kp: 1.0 + ki: 0.0 + kd: 0.0 + +broadcaster: + ros__parameters: + forward_list: + - /dart/drive_belt/left/control_torque + - /dart/drive_belt/right/control_torque \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index ee4f271e..486c1526 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -19,7 +19,7 @@ dart_hardware: dart_manager: ros__parameters: max_transform_rate: 15.0 - manual_force_scale: 15.0 + manual_force_scale: 5.0 remote_cmd_bridge: ros__parameters: @@ -32,7 +32,7 @@ launch_setting: max_control_torque: 15.0 trigger_free_value: 0.75 trigger_lock_value: 0.63 - b_kp: 1.0 + b_kp: 2.0 b_ki: 0.0 b_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml deleted file mode 100644 index 7a4b9ac3..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-launch.yaml +++ /dev/null @@ -1,68 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Full -> dart_hardware - - rmcs_core::controller::dart::DartLaunchControllerV2 -> launch_controller - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -launch_controller: - ros__parameters: - belt_velocity: 15.0 - sync_coefficient: 0.5 - max_control_torque: 15.0 - b_kp: 1.0 - b_ki: 0.0 - b_kd: 0.0 - # Trigger servo PWM value 0.0–1.0 (from dart-filling-test calibration) - trigger_free_value: 0.75 - trigger_lock_value: 0.63 - -dart_settings: - ros__parameters: - # 0 = manual joystick, 1 = auto PID - is_auto_pitch_control_mode: 0 - is_auto_force_control_mode: 0 - max_transform_rate: 5.0 - pitch_angle_kp: 0.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - force_control_kp: 0.001 - force_control_ki: 0.0 - force_control_kd: 0.0 - -force_screw_velocity_pid_controller: - ros__parameters: - measurement: /dart/force_screw_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_screw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /dart/yaw_motor/velocity - setpoint: /yaw/control/velocity - control: /dart/yaw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /dart/pitch_motor/velocity - setpoint: /pitch/control/velocity - control: /dart/pitch_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml deleted file mode 100644 index 12777b5b..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-pitch-pid.yaml +++ /dev/null @@ -1,98 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDart -> catapult_dart - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::controller::dart::DartLaunchController -> launch_controller - - rmcs_core::controller::dart::Test -> actual_speed_and_control_torque_test_node - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> force_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> Value_Broadcasters - -catapult_dart: - ros__parameters: - usb_pid: -1 - first_sample_spot: 1.0 - final_sample_spot: 4.0 - trigger_runtime: 0x0000 - limiting_runtime: 0x0000 - lifting_left_runtime: 0x0000 - lifting_right_runtime: 0x0000 - -dart_settings: - ros__parameters: - is_auto_pitch_control_mode: 0 #<0 = false = manual_control_mode> or <1 = true = double_loop_pid> - is_auto_force_control_mode: 0 #<0 = false = manual_control_mode> or <1 = true = force_sensor> - pitch_angle_kp: 1.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - force_control_kp: 0.5 - force_control_ki: 0.0 - force_control_kd: 0.0 - max_transform_rate: 20.0 - -launch_controller: - ros__parameters: - belt_velocity: 10.0 - lifting_velocity: 10.0 - sync_coefficient: 0.5 - lifting_sync_coefficient: 0.5 - b_kp: 1.0 - b_ki: 0.0 - b_kd: 0.0 - max_control_torque: 2.5 - trigger_free_angle: 0x0E2A - trigger_lock_angle: 0x0FC1 - lifting_up_angle_left: 0x06D7 - lifting_down_angle_left: 0x0DAA - lifting_up_angle_right: 0x08C9 - lifting_down_angle_right: 0x0C05 - limiting_wait_time: 0x0001 - limiting_free_angle_: 0x0001 - limiting_lock_angle_: 0x0001 - - -actual_speed_and_control_torque_test_node: - ros__parameters: - lifting_up_angle_left: 0x06D7 - lifting_down_angle_left: 0x0DAA - lifting_up_angle_right: 0x08C9 - lifting_down_angle_right: 0x0B05 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /dart/pitch_motor/velocity - setpoint: /pitch/control/velocity - control: /dart/pitch_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /dart/yaw_motor/velocity - setpoint: /yaw/control/velocity - control: /dart/yaw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -force_pid_controller: - ros__parameters: - measurement: /dart/force_control_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_control_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -Value_Broadcasters: - ros__parameters: - forward_list: - # - "/dart/pitch/angle" - - "/dart/force_control_motor/velocity" - - "/imu/catapult_roll_angle" - - "/imu/catapult_pitch_angle" - - "/imu/catapult_yaw_angle" \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml new file mode 100644 index 00000000..d0ed8157 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml @@ -0,0 +1,16 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + - rmcs_core::hardware::CatapultDartV4 -> dart_hardware + +remote_cmd_bridge: + ros__parameters: + joystick_sensitivity: 0.01 + +dart_hardware: + ros__parameters: + serial_filter: "" + first_sample_spot: 1.0 + final_sample_spot: 4.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart.yaml b/rmcs_ws/src/rmcs_bringup/config/dart.yaml deleted file mode 100644 index 514b50ec..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart.yaml +++ /dev/null @@ -1,91 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Full -> dart_hardware - - rmcs_core::controller::dart::DartLaunchController -> launch_controller - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster - -broadcaster: - ros__parameters: - forward_list: - - /force/control/velocity - - /dart/force_screw_motor/velocity - - /dart/drive_belt/left/control_torque - - /dart/drive_belt/left/velocity - - /dart/drive_belt/right/control_torque - - /dart/drive_belt/right/velocity - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -launch_controller: - ros__parameters: - belt_velocity: 10.0 - sync_coefficient: 0.1 - b_kp: 1.0 - b_ki: 0.0 - b_kd: 0.0 - max_control_torque: 15.0 - # Trigger servo: PWM value 0.0–1.0 (PWMServo: 0.5ms–2.5ms in 20ms period) - trigger_free_value: 0.6 - trigger_lock_value: 0.4 - # Lifting UART servo angles - lifting_up_angle_left: 0x0656 - lifting_down_angle_left: 0x0DE3 - lifting_up_angle_right: 0x08C7 - lifting_down_angle_right: 0x0C54 - # Limiting servo angles - limiting_wait_time: 16 - limiting_free_angle_: 0x0001 - limiting_lock_angle_: 0x0050 - -dart_settings: - ros__parameters: - # 0 = manual joystick, 1 = auto PID - is_auto_pitch_control_mode: 0 - is_auto_force_control_mode: 0 - max_transform_rate: 5.0 - # Pitch angle PID (used when is_auto_pitch_control_mode = 1) - pitch_angle_kp: 0.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - # Force control PID (used when is_auto_force_control_mode = 1) - force_control_kp: 0.001 - force_control_ki: 0.0 - force_control_kd: 0.0 - -# dart_settings outputs /force/control/velocity as the velocity setpoint for the screw motor -force_screw_velocity_pid_controller: - ros__parameters: - measurement: /dart/force_screw_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_screw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -yaw_velocity_pid_controller: - ros__parameters: - measurement: /dart/yaw_motor/velocity - setpoint: /yaw/control/velocity - control: /dart/yaw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -pitch_velocity_pid_controller: - ros__parameters: - measurement: /dart/pitch_motor/velocity - setpoint: /pitch/control/velocity - control: /dart/pitch_motor/control_torque - kp: 0.0 - ki: 0.0 - kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 23162a9a..727accdf 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -152,4 +152,7 @@ Dart setting controller: subscribes to RemoteCommandBridge delta commands, outputs yaw/pitch/force control velocities. + + Launch setting V2: belt + trigger + LK lifting angle control. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp index 690b7c0b..8a0d9ac8 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp @@ -53,6 +53,9 @@ class DartFillingTest register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); + register_input("/dart/lifting_left/angle", lifting_left_angle_); + register_input("/dart/lifting_right/angle", lifting_right_angle_); + // Trigger servo: PWM double value register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); @@ -63,8 +66,11 @@ class DartFillingTest register_output("/dart/drive_belt/left/control_torque", belt_left_torque_, 0.0); register_output("/dart/drive_belt/right/control_torque", belt_right_torque_, 0.0); + register_output("/dart/lifting_left/control_velocity", lifting_left_angle_shift_); + register_output("/dart/lifting_right/control_velocity", lifting_right_angle_shift_); register_output("/dart/filling/stage", filling_stage_); register_output("/force/sensor/average", average_force_); + register_output("/dart/limiting_servo/control_angle", limiting_control_angle_); timer_ = this->create_wall_timer( std::chrono::milliseconds(timer_interval_ms_), @@ -89,9 +95,9 @@ class DartFillingTest *pitch_torque_ = std::clamp(joystick_right_->x() * max_torque_, -max_torque_, max_torque_); - double belt_cmd = joystick_right_->y() * max_belt_torque_; - *belt_left_torque_ = std::clamp(belt_cmd, -max_belt_torque_, max_belt_torque_); - *belt_right_torque_ = std::clamp(belt_cmd, -max_belt_torque_, max_belt_torque_); + double lifting_cmd = joystick_right_->y() * max_belt_torque_; + *lifting_left_angle_shift_ = lifting_cmd; + *lifting_right_angle_shift_ = -lifting_cmd; *force_screw_torque_ = 0.0; @@ -118,8 +124,11 @@ class DartFillingTest log_count_ = 0; RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); + RCLCPP_INFO(logger_, "[Lifting] left: %.4f rad right: %.4f rad", + *lifting_left_angle_, *lifting_right_angle_); } + *limiting_control_angle_ = 0; last_switch_left_ = sw_left; last_switch_right_ = sw_right; } @@ -178,7 +187,13 @@ class DartFillingTest OutputInterface force_screw_torque_; OutputInterface belt_left_torque_; OutputInterface belt_right_torque_; + OutputInterface lifting_left_angle_shift_; + OutputInterface lifting_right_angle_shift_; + // LK lifting motor angle feedback (radians, multi-turn) + InputInterface lifting_left_angle_; + InputInterface lifting_right_angle_; + OutputInterface limiting_control_angle_; OutputInterface average_force_; // Stage output (used by broadcaster) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp index 4623598c..e8443491 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp @@ -31,6 +31,7 @@ class DartSettingController register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); + } void update() override { diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp deleted file mode 100644 index 0c10e5bd..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_settings.cpp +++ /dev/null @@ -1,185 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include "controller/pid/pid_calculator.hpp" -#include - -/* -angle controls & launch parameter settings -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -enum class SwitchMode : uint8_t { - LOCKED =0, - UNLOCKED =1 -}; - -class DartSettings - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartSettings() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - // register_input("/dart_guidance/angle/error", yaw_pitch_angle_); - register_output("/yaw/control/velocity", yaw_control_velocity_); - register_output("/force/control/velocity", force_control_); - register_output("/pitch/control/velocity", pitch_control_velocity_); - register_output("/force/sensor/average", average_force_); - is_auto_pitch_control_mode_ = get_parameter("is_auto_pitch_control_mode").as_int(); - is_auto_force_control_mode_ = get_parameter("is_auto_force_control_mode").as_int(); - pitch_angle_kp_ = get_parameter("pitch_angle_kp").as_double(); - pitch_angle_ki_ = get_parameter("pitch_angle_ki").as_double(); - pitch_angle_kd_ = get_parameter("pitch_angle_kd").as_double(); - force_control_kp_ = get_parameter("force_control_kp").as_double(); - force_control_ki_ = get_parameter("force_control_ki").as_double(); - force_control_kd_ = get_parameter("force_control_kd").as_double(); - max_transform_rate_ = get_parameter("max_transform_rate").as_double(); - } - - void before_updating() override { - if (!switch_left_.ready()) { - RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_left\". Set to 0.0."); - } - if (!switch_right_.ready()) { - RCLCPP_WARN(get_logger(), "Failed to fetch \"/switch_right\". Set to 0.0."); - } - } - - void update() override { - using namespace rmcs_msgs; - - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - - do {if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::UP && switch_right == Switch::MIDDLE)) { - if (is_auto_pitch_control_mode_ == 0) { // manual control mode - *yaw_control_velocity_ = joystick_left_->y() * max_transform_rate_; - *pitch_control_velocity_ = joystick_right_->x() * max_transform_rate_; - break; - } else if (is_auto_pitch_control_mode_ == 1){ // double loop pid - // double yaw_angle = (*yaw_pitch_angle_)[0]; - // *yaw_control_velocity_ = yaw_angle * max_transform_rate_; - pitch_control(); - break; - } - } - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::UP && switch_right == Switch::DOWN)) { - if (is_auto_force_control_mode_ == 0) { - *force_control_ = joystick_right_->x() * max_transform_rate_ * 5; - break; - } else if (is_auto_force_control_mode_ == 1) { - transmit_distance_to_force(); - force_control(); - } - } - } while(false); - - // Force sensor: always update average and log periodically for verification - *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; - log_count_++; - if (log_count_ >= 100) { - log_count_ = 0; - RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", - *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); - } - - // RCLCPP_INFO(this->get_logger(), "yaw_control_speed = %f, pitch_control_speed = %f, force_control_speed = %f", *yaw_control_velocity_, *pitch_control_velocity_, *force_control_); - - }; - -private: - void reset_all_controls() { - *yaw_control_velocity_ = nan; - *pitch_control_velocity_ = nan; - *force_control_ = nan; - } - - void pitch_control() { - pitch_angle_setpoint_ = joystick_right_->x() * max_transform_rate_; - pitch_angle_pid_controller.kp = pitch_angle_kp_; - pitch_angle_pid_controller.ki = pitch_angle_ki_; - pitch_angle_pid_controller.kd = pitch_angle_kd_; - // double pitch_angle = (*yaw_pitch_angle_)[1]; - // *pitch_control_velocity_ = pitch_angle_pid_controller.update(pitch_angle_setpoint_ - pitch_angle); - } - - void force_control() { - force_control_pid_controller.kp = force_control_kp_; - force_control_pid_controller.ki = force_control_ki_; - force_control_pid_controller.kd = force_control_kd_; - *force_control_ = force_control_pid_controller.update(force_control_setpoint_ - *average_force_); - } - - void transmit_distance_to_force() { - force_control_setpoint_ = 3852.0; - } - -private: - static constexpr double nan = std::numeric_limits::quiet_NaN(); - rclcpp::Logger logger_; - - int log_count_ = 0; - int64_t is_auto_pitch_control_mode_; - int64_t is_auto_force_control_mode_; - double max_transform_rate_; - - double pitch_angle_setpoint_; - double force_control_setpoint_; - - pid::PidCalculator pitch_angle_pid_controller; - pid::PidCalculator force_control_pid_controller; - - double pitch_angle_kp_, pitch_angle_ki_, pitch_angle_kd_; - double force_control_kp_, force_control_ki_, force_control_kd_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; - // InputInterface yaw_pitch_angle_; - - OutputInterface yaw_control_velocity_; - OutputInterface force_control_; - OutputInterface pitch_control_velocity_; - OutputInterface average_force_; - -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp deleted file mode 100644 index f6ba4671..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller.cpp +++ /dev/null @@ -1,325 +0,0 @@ -// #include "controller/pid/matrix_pid_calculator.hpp" -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// /* -// launch controls -// 键位： -// 双下：全部停止 -// 双中：初始状态 -// 此时{ -// 右拨杆下拨再回中：切换上膛和退膛 -// 处于上膛状态时右拨杆打到上：发射 -// } -// 左拨杆上：设置模式 -// 此时{ -// 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 -// 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 -// } -// */ - -// namespace rmcs_core::controller::dart { -// class DartLaunchController -// : public rmcs_executor::Component -// , public rclcpp::Node { -// public: -// DartLaunchController() -// : Node{ -// get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , timer_interval_ms_(10) -// , logger_(get_logger()) -// , drive_belt_pid_calculator_( -// get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), -// get_parameter("b_kd").as_double()) { - -// drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); -// sync_coefficient_ = get_parameter("sync_coefficient").as_double(); -// max_control_torque_ = get_parameter("max_control_torque").as_double(); - -// launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); -// launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); - -// lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); -// lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - -// lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); -// lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - -// limiting_wait_time_ = get_parameter("limiting_wait_time").as_int(); -// limiting_trigger_angle_ = get_parameter("limiting_free_angle_").as_int(); -// limiting_lock_angle_ = get_parameter("limiting_lock_angle_").as_int(); - -// register_input("/remote/joystick/right", joystick_right_); -// register_input("/remote/joystick/left", joystick_left_); -// register_input("/remote/switch/right", switch_right_); -// register_input("/remote/switch/left", switch_left_); - -// register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); -// register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - -// register_input("/dart/lifting_left/current_angle", lifting_angle_left_); -// register_input("/dart/lifting_right/current_angle", lifting_angle_right_); - -// register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); -// register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); - -// register_output("/dart/trigger_servo/value", trigger_control_angle); -// register_output("/dart/limiting_servo/control_angle", limiting_control_angle); -// register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); -// register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); - -// register_output("/dart/filling/stage", filling_stage_); - -// timer_ = this->create_wall_timer( -// std::chrono::milliseconds(timer_interval_ms_), -// [this] { timer_callback(); }); - -// // *lifting_left_control_angle = lifting_up_angle_left_; -// // *lifting_right_control_angle = lifting_up_angle_right_; -// } - -// void update() override { -// using namespace rmcs_msgs; - -// if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) -// && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { -// *launch_stage_ = DartLaunchStages::DISABLE; -// *filling_stage_ = DartFillingStages::INIT; -// reset_all_controls(); - -// } else if (*switch_left_ == Switch::MIDDLE) { - -// if (last_launch_stage_ == DartLaunchStages::DISABLE) { -// *launch_stage_ = DartLaunchStages::RESETTING; -// // *filling_stage_ = DartFillingStages::FILLING; // assume that we already have a dart on the lifting platform at the beginning -// } - -// if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { -// if (last_launch_stage_ == DartLaunchStages::INIT) { -// *launch_stage_ = DartLaunchStages::LOADING; -// } else if (last_launch_stage_ == DartLaunchStages::READY) { -// *lifting_left_control_angle = lifting_up_angle_left_; -// *lifting_right_control_angle = lifting_up_angle_right_; -// *launch_stage_ = DartLaunchStages::CANCEL; -// } -// } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { -// if (last_launch_stage_ == DartLaunchStages::READY) { -// *launch_stage_ = DartLaunchStages::INIT; -// trigger_lock_flag_ = false; -// delay_and_execute(20, [this]() { -// *lifting_left_control_angle = lifting_up_angle_left_; -// *lifting_right_control_angle = lifting_up_angle_right_; -// *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; -// }); -// delay_and_execute(500, [this]() { -// loading_process(); -// }); -// } else { -// RCLCPP_INFO(logger_, "Dart has't been loaded !"); -// } -// } - -// if (blocking_detection()) { -// if (last_launch_stage_ == DartLaunchStages::LOADING) { -// *lifting_left_control_angle = lifting_down_angle_left_; -// *lifting_right_control_angle = lifting_down_angle_right_; -// *filling_stage_ = rmcs_msgs::DartFillingStages::DOWNING; -// delay_and_execute(500, [this]() { -// *launch_stage_ = DartLaunchStages::RESETTING; -// *filling_stage_ = rmcs_msgs::DartFillingStages::READY; -// trigger_lock_flag_ = true; -// drive_belt_block_count_ = 0; -// }); -// // if (*lifting_angle_left_ == lifting_down_angle_left_ && *lifting_angle_right_ == lifting_down_angle_right_) { - -// // *launch_stage_ = DartLaunchStages::RESETTING; -// // *filling_stage_ = rmcs_msgs::DartFillingStages::READY; -// // trigger_lock_flag_ = true; -// // drive_belt_block_count_ = 0; -// // } -// } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { -// *lifting_left_control_angle = lifting_up_angle_left_; -// *lifting_right_control_angle = lifting_up_angle_right_; -// *filling_stage_ = rmcs_msgs::DartFillingStages::LIFTING; -// delay_and_execute(500, [this]() { -// *launch_stage_ = DartLaunchStages::RESETTING; -// *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; -// trigger_lock_flag_ = false; -// drive_belt_block_count_ = 0; -// }); -// // if (*lifting_angle_left_ == lifting_up_angle_left_ && *lifting_angle_right_ == lifting_up_angle_right_) { -// // *launch_stage_ = DartLaunchStages::RESETTING; -// // *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; -// // trigger_lock_flag_ = false; -// // drive_belt_block_count_ = 0; -// // }; -// } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { -// *launch_stage_ = -// trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; -// *filling_stage_ = -// trigger_lock_flag_ ? DartFillingStages::READY : DartFillingStages::INIT; -// drive_belt_block_count_ = 0; -// } -// } -// double control_velocity = 0; - -// if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { -// control_velocity = -drive_belt_working_velocity_; -// } else if ( -// *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING -// || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { -// control_velocity = drive_belt_working_velocity_; -// } else { -// control_velocity = 0; -// } -// drive_belt_sync_control(control_velocity); -// // RCLCPP_INFO(logger_, "%lf", control_velocity); -// } - -// *trigger_control_angle = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; - -// last_switch_left_ = *switch_left_; -// last_switch_right_ = *switch_right_; -// last_launch_stage_ = *launch_stage_; -// } - -// private: -// void reset_all_controls() { -// *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; -// *left_drive_belt_control_torque_ = 0; -// *right_drive_belt_control_torque_ = 0; -// } - -// void drive_belt_sync_control(double set_velocity) { -// if (set_velocity == 0) { -// *left_drive_belt_control_torque_ = 0; -// *right_drive_belt_control_torque_ = 0; -// return; -// } - -// Eigen::Vector2d setpoint_error{ -// set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; - -// Eigen::Vector2d relative_velocity{ -// *left_drive_belt_velocity_ - *right_drive_belt_velocity_, -// *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - -// Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - -// *left_drive_belt_control_torque_ = -// std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); -// *right_drive_belt_control_torque_ = -// std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); -// } - -// bool blocking_detection() { -// if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) -// || (abs(*right_drive_belt_velocity_) < 0.5 -// && abs(*right_drive_belt_control_torque_) > 0.5)) { -// drive_belt_block_count_++; -// } - -// return drive_belt_block_count_ > 1000 ? true : false; -// } - -// void loading_process() { -// *limiting_control_angle = limiting_trigger_angle_; -// *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; -// delay_and_execute(2000, [this]() { -// *limiting_control_angle = limiting_lock_angle_; -// *filling_stage_ = rmcs_msgs::DartFillingStages::INIT; -// }); -// } - -// rclcpp::TimerBase::SharedPtr timer_; -// int timer_interval_ms_; -// std::function delayed_action_; -// bool is_delaying_ = false; -// int delay_remaining_ms_ = 0; - -// void timer_callback() { -// if (is_delaying_ && delay_remaining_ms_ > 0) { -// delay_remaining_ms_ -= timer_interval_ms_; -// if (delay_remaining_ms_ <= 0) { -// is_delaying_ = false; -// if (delayed_action_) { -// delayed_action_(); -// } -// } -// } -// } - -// void delay_and_execute(int delay_ms, std::function action) { -// if (!is_delaying_) { -// is_delaying_ = true; -// delay_remaining_ms_ = delay_ms; -// delayed_action_ = std::move(action); -// } -// } - -// int drive_belt_block_count_ = 0; -// double drive_belt_working_velocity_; - -// rclcpp::Logger logger_; - -// InputInterface joystick_right_; -// InputInterface joystick_left_; -// InputInterface switch_right_; -// InputInterface switch_left_; -// rmcs_msgs::Switch last_switch_right_; -// rmcs_msgs::Switch last_switch_left_; - -// InputInterface lifting_angle_left_; -// InputInterface lifting_angle_right_; -// OutputInterface left_drive_belt_control_torque_; -// OutputInterface right_drive_belt_control_torque_; -// InputInterface left_drive_belt_velocity_; -// InputInterface right_drive_belt_velocity_; - -// double max_control_torque_; - -// OutputInterface launch_stage_; -// rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - -// double launch_lock_value_; -// double launch_trigger_value_; -// bool trigger_lock_flag_ = false; -// OutputInterface trigger_control_angle; - -// OutputInterface filling_stage_; -// OutputInterface pulse_sending_flag_; - -// uint16_t lifting_up_angle_left_; -// uint16_t lifting_down_angle_left_; -// uint16_t lifting_up_angle_right_; -// uint16_t lifting_down_angle_right_; - -// OutputInterface lifting_left_control_angle; -// OutputInterface lifting_right_control_angle; - -// uint16_t limiting_lock_angle_; -// uint16_t limiting_trigger_angle_; -// uint16_t limiting_wait_time_; - -// OutputInterface limiting_control_angle; - -// pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; -// double sync_coefficient_; -// }; - -// } // namespace rmcs_core::controller::dart - -// #include -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp deleted file mode 100644 index a90b1aec..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2.cpp +++ /dev/null @@ -1,489 +0,0 @@ -// #include "controller/pid/matrix_pid_calculator.hpp" -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// /* -// launch controls -// 键位： -// 双下：全部停止 -// 双中：初始状态 -// 此时{ -// 右拨杆下拨再回中：切换上膛和退膛 -// 处于上膛状态时右拨杆打到上：发射 -// } -// 左拨杆上：设置模式 -// 此时{ -// 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 -// 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 -// } -// */ - -// namespace rmcs_core::controller::dart { -// class DartLaunchControllerV2 -// : public rmcs_executor::Component -// , public rclcpp::Node { -// public: -// DartLaunchControllerV2() -// : Node{ -// get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , timer_interval_ms_(10) -// , logger_(get_logger()) -// , drive_belt_pid_calculator_( -// get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), -// get_parameter("b_kd").as_double()) { - -// drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); -// sync_coefficient_ = get_parameter("sync_coefficient").as_double(); -// max_control_torque_ = get_parameter("max_control_torque").as_double(); - -// trigger_free_ = get_parameter("trigger_free_value").as_double(); -// trigger_lock_ = get_parameter("trigger_lock_value").as_double(); -// register_input("/dart/trigger_flag", trigger_flag_); -// register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); -// register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - -// register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); -// register_output("/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); - -// register_output("/dart/trigger_servo/value", trigger_control_angle); - - -// timer_ = this->create_wall_timer( -// std::chrono::milliseconds(timer_interval_ms_), -// [this] { timer_callback(); }); - -// } - -// void update() override { -// using namespace rmcs_msgs; - -// if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) -// && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { -// *launch_stage_ = DartLaunchStages::DISABLE; -// reset_all_controls(); - -// } else if (*switch_left_ == Switch::MIDDLE) { - -// if (last_launch_stage_ == DartLaunchStages::DISABLE) { -// *launch_stage_ = DartLaunchStages::RESETTING; -// } - -// if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { -// if (last_launch_stage_ == DartLaunchStages::INIT) { -// *launch_stage_ = DartLaunchStages::LOADING; -// trigger_lock_flag_ = false; -// } else if (last_launch_stage_ == DartLaunchStages::READY) { -// *launch_stage_ = DartLaunchStages::CANCEL; -// } -// } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { -// if (last_launch_stage_ == DartLaunchStages::READY) { -// *launch_stage_ = DartLaunchStages::INIT; -// trigger_lock_flag_ = false; -// } else { -// RCLCPP_INFO(logger_, "Dart has't been loaded !"); -// } -// } - - -// if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { -// control_velocity = -drive_belt_working_velocity_; -// } else if ( -// *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING -// || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { -// control_velocity = drive_belt_working_velocity_; -// } else { -// control_velocity = 0; -// } -// drive_belt_sync_control(control_velocity); -// } - -// *trigger_control_angle = *trigger_flag_ ? trigger_lock_ : trigger_free_; - -// } - -// private: -// void reset_all_controls() { -// *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; -// *left_drive_belt_control_torque_ = 0; -// *right_drive_belt_control_torque_ = 0; -// } - -// void drive_belt_sync_control(double set_velocity) { -// if (set_velocity == 0) { -// *left_drive_belt_control_torque_ = 0; -// *right_drive_belt_control_torque_ = 0; -// return; -// } - -// Eigen::Vector2d setpoint_error{ -// set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; - -// Eigen::Vector2d relative_velocity{ -// *left_drive_belt_velocity_ - *right_drive_belt_velocity_, -// *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - -// Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - -// *left_drive_belt_control_torque_ = -// std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); -// *right_drive_belt_control_torque_ = -// std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); -// } - -// rclcpp::TimerBase::SharedPtr timer_; -// int timer_interval_ms_; -// std::function delayed_action_; -// bool is_delaying_ = false; -// int delay_remaining_ms_ = 0; - -// void timer_callback() { -// if (is_delaying_ && delay_remaining_ms_ > 0) { -// delay_remaining_ms_ -= timer_interval_ms_; -// if (delay_remaining_ms_ <= 0) { -// is_delaying_ = false; -// if (delayed_action_) { -// delayed_action_(); -// } -// } -// } -// } - -// void delay_and_execute(int delay_ms, std::function action) { -// if (!is_delaying_) { -// is_delaying_ = true; -// delay_remaining_ms_ = delay_ms; -// delayed_action_ = std::move(action); -// } -// } - -// double control_velocity; -// double drive_belt_working_velocity_; - -// rclcpp::Logger logger_; - -// InputInterface trigger_flag_; -// InputInterface left_drive_belt_velocity_; -// InputInterface right_drive_belt_velocity_; - -// OutputInterface left_drive_belt_control_torque_; -// OutputInterface right_drive_belt_control_torque_; - -// double max_control_torque_; - -// OutputInterface launch_stage_; - -// double trigger_free_; -// double trigger_lock_; -// OutputInterface trigger_control_angle; - -// pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; -// double sync_coefficient_; -// }; - -// } // namespace rmcs_core::controller::dart - -// #include -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchControllerV2, rmcs_executor::Component) - - -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include -// // // #include - -// // // /* -// // // DartLaunchControllerV2 — 发射控制 Phase 1（忽略升降电机与限位舵机） -// // // 仅控制传送带与扳机舵机，不涉及升降平台和限位舵机。 - -// // // 键位： -// // // 双下 / UNKNOWN：DISABLE，停止所有输出 - -// // // 左拨杆中：发射控制模式 -// // // 进入后自动进入 RESETTING（传送带反转，堵转后 → INIT） -// // // INIT + 右拨杆中→下：开始上膛（LOADING，传送带正转，堵转后 → READY） -// // // READY + 右拨杆中→上：发射（trigger FREE，延时后 LOCK，→ RESETTING） -// // // READY + 右拨杆中→下：取消（trigger LOCK，→ RESETTING） - -// // // 左拨杆上：DartSettings 角度/力矩设置模式（launch controller 不干预） -// // // */ - -// // // namespace rmcs_core::controller::dart { - -// // // class DartLaunchControllerV2 -// // // : public rmcs_executor::Component -// // // , public rclcpp::Node { -// // // public: -// // // DartLaunchControllerV2() -// // // : Node{get_component_name(), -// // // rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// // // , timer_interval_ms_(10) -// // // , logger_(get_logger()) { - -// // // drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); -// // // sync_coefficient_ = get_parameter("sync_coefficient").as_double(); -// // // max_control_torque_ = get_parameter("max_control_torque").as_double(); -// // // launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); -// // // launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); -// // // trigger_free_duration_ms_ = get_parameter("trigger_free_duration_ms").as_double(); - -// // // // Compatibility outputs: keep servos in safe position (Phase 1 does not actively control) -// // // lifting_up_angle_left_ = static_cast( -// // // get_parameter("lifting_up_angle_left").as_int()); -// // // lifting_up_angle_right_ = static_cast( -// // // get_parameter("lifting_up_angle_right").as_int()); -// // // limiting_lock_angle_ = static_cast( -// // // get_parameter("limiting_lock_angle_").as_int()); - -// // // register_input("/remote/switch/right", switch_right_); -// // // register_input("/remote/switch/left", switch_left_); -// // // register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); -// // // register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - -// // // register_output( -// // // "/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0.0); -// // // register_output( -// // // "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0.0); -// // // register_output("/dart/trigger_servo/value", trigger_value_, launch_lock_value_); - -// // // register_output("/dart/filling/stage", filling_stage_); - -// // // // Required by CatapultDartV3Full hardware (TriggerServo mandatory inputs) -// // // register_output("/dart/limiting_servo/control_angle", limiting_control_angle_, -// // // limiting_lock_angle_); -// // // register_output("/dart/lifting_left/control_angle", lifting_left_control_angle_, -// // // lifting_up_angle_left_); -// // // register_output("/dart/lifting_right/control_angle", lifting_right_control_angle_, -// // // lifting_up_angle_right_); - -// // // timer_ = this->create_wall_timer( -// // // std::chrono::milliseconds(timer_interval_ms_), -// // // [this] { timer_callback(); }); -// // // } - -// // // void update() override { -// // // using namespace rmcs_msgs; - -// // // auto sw_left = *switch_left_; -// // // auto sw_right = *switch_right_; - -// // // const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) -// // // && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); - -// // // if (all_down) { -// // // // SAFETY: stop everything and LOCK trigger immediately. -// // // // reset_all_controls() cancels pending delays and stops belt. -// // // // Trigger is set directly here — never depends on trigger_lock_flag_. -// // // reset_all_controls(); -// // // *trigger_value_ = launch_lock_value_; - -// // // } else if (sw_left == Switch::MIDDLE) { - -// // // // Transition out of DISABLE when entering MIDDLE -// // // if (stage_ == DartLaunchStages::DISABLE) { -// // // stage_ = DartLaunchStages::RESETTING; -// // // drive_belt_block_count_ = 0; -// // // RCLCPP_INFO(logger_, "DISABLE → RESETTING"); -// // // } - -// // // // Switch edge: right MIDDLE → DOWN -// // // if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { -// // // if (stage_ == DartLaunchStages::INIT) { -// // // stage_ = DartLaunchStages::LOADING; -// // // drive_belt_block_count_ = 0; -// // // RCLCPP_INFO(logger_, "INIT → LOADING"); -// // // } else if (stage_ == DartLaunchStages::READY) { -// // // // Cancel without firing. -// // // // Keep trigger_lock_flag_ = true so trigger stays LOCKED -// // // // while belt reverses to reset position (RESETTING → INIT). -// // // stage_ = DartLaunchStages::RESETTING; -// // // drive_belt_block_count_ = 0; -// // // RCLCPP_INFO(logger_, "READY → RESETTING (cancel, trigger stays LOCKED)"); -// // // } - -// // // // Switch edge: right MIDDLE → UP -// // // } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { -// // // if (stage_ == DartLaunchStages::READY) { -// // // // Fire: release trigger, stop belt during fire. -// // // trigger_lock_flag_ = false; -// // // stage_ = DartLaunchStages::INIT; -// // // *filling_stage_ = DartFillingStages::INIT; -// // // RCLCPP_INFO(logger_, "READY → LAUNCH (trigger free for %.0f ms)", -// // // trigger_free_duration_ms_); -// // // delay_and_execute(static_cast(trigger_free_duration_ms_), [this]() { -// // // // Dart has fired: re-lock trigger and start resetting belt. -// // // trigger_lock_flag_ = true; -// // // stage_ = DartLaunchStages::RESETTING; -// // // drive_belt_block_count_ = 0; -// // // RCLCPP_INFO(logger_, "LAUNCH → RESETTING"); -// // // }); -// // // } else { -// // // RCLCPP_WARN(logger_, "Cannot launch: not in READY state (current: %d)", -// // // static_cast(stage_)); -// // // } -// // // } - -// // // // Blocking detection → automatic stage transitions -// // // if (blocking_detection()) { -// // // if (stage_ == DartLaunchStages::RESETTING) { -// // // // Spring/belt fully reset, no dart loaded -// // // stage_ = DartLaunchStages::INIT; -// // // trigger_lock_flag_ = false; -// // // drive_belt_block_count_ = 0; -// // // RCLCPP_INFO(logger_, "RESETTING → INIT (blocked)"); -// // // } else if (stage_ == DartLaunchStages::LOADING) { -// // // // Spring fully compressed, dart loaded -// // // stage_ = DartLaunchStages::READY; -// // // trigger_lock_flag_ = true; -// // // drive_belt_block_count_ = 0; -// // // *filling_stage_ = DartFillingStages::READY; -// // // RCLCPP_INFO(logger_, "LOADING → READY (blocked)"); -// // // } -// // // } - -// // // // Belt velocity based on current stage -// // // double control_velocity = 0.0; -// // // if (stage_ == DartLaunchStages::RESETTING) { -// // // control_velocity = -drive_belt_working_velocity_; -// // // } else if (stage_ == DartLaunchStages::LOADING) { -// // // control_velocity = drive_belt_working_velocity_; -// // // } -// // // drive_belt_sync_control(control_velocity); - -// // // // Trigger: updated only in MIDDLE mode -// // // *trigger_value_ = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; - -// // // } -// // // // LEFT_UP (DartSettings mode): trigger maintains its last written value, no change. - -// // // last_switch_left_ = sw_left; -// // // last_switch_right_ = sw_right; -// // // } - -// // // private: -// // // void reset_all_controls() { -// // // stage_ = rmcs_msgs::DartLaunchStages::DISABLE; -// // // *left_drive_belt_control_torque_ = 0.0; -// // // *right_drive_belt_control_torque_ = 0.0; -// // // // Cancel any pending delayed action (e.g. a mid-launch or post-launch timer) -// // // is_delaying_ = false; -// // // delayed_action_ = nullptr; -// // // trigger_lock_flag_ = false; -// // // drive_belt_block_count_ = 0; -// // // // NOTE: *trigger_value_ is written by the caller (all_down branch) after this returns. -// // // } - -// // // void drive_belt_sync_control(double set_velocity) { -// // // if (set_velocity == 0.0) { -// // // *left_drive_belt_control_torque_ = 0.0; -// // // *right_drive_belt_control_torque_ = 0.0; -// // // return; -// // // } - -// // // Eigen::Vector2d setpoint_error{ -// // // set_velocity - *left_drive_belt_velocity_, -// // // set_velocity - *right_drive_belt_velocity_}; -// // // Eigen::Vector2d relative_velocity{ -// // // *left_drive_belt_velocity_ - *right_drive_belt_velocity_, -// // // *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; -// // // Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - -// // // *left_drive_belt_control_torque_ = -// // // std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); -// // // *right_drive_belt_control_torque_ = -// // // std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); -// // // } - -// // // bool blocking_detection() { -// // // if ((std::abs(*left_drive_belt_velocity_) < 0.5 -// // // && std::abs(*left_drive_belt_control_torque_) > 0.5) -// // // || (std::abs(*right_drive_belt_velocity_) < 0.5 -// // // && std::abs(*right_drive_belt_control_torque_) > 0.5)) { -// // // drive_belt_block_count_++; -// // // } -// // // return drive_belt_block_count_ > 1000; -// // // } - -// // // rclcpp::TimerBase::SharedPtr timer_; -// // // int timer_interval_ms_; -// // // std::function delayed_action_; -// // // bool is_delaying_ = false; -// // // int delay_remaining_ms_ = 0; - -// // // void timer_callback() { -// // // if (is_delaying_ && delay_remaining_ms_ > 0) { -// // // delay_remaining_ms_ -= timer_interval_ms_; -// // // if (delay_remaining_ms_ <= 0) { -// // // is_delaying_ = false; -// // // if (delayed_action_) -// // // delayed_action_(); -// // // } -// // // } -// // // } - -// // // // delay_ms accepts double from yaml (e.g. trigger_free_duration_ms_); cast to int internally. -// // // void delay_and_execute(int delay_ms, std::function action) { -// // // if (!is_delaying_) { -// // // is_delaying_ = true; -// // // delay_remaining_ms_ = delay_ms; -// // // delayed_action_ = std::move(action); -// // // } -// // // } - -// // // rclcpp::Logger logger_; - -// // // InputInterface switch_right_; -// // // InputInterface switch_left_; -// // // rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; -// // // rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - -// // // InputInterface left_drive_belt_velocity_; -// // // InputInterface right_drive_belt_velocity_; -// // // OutputInterface left_drive_belt_control_torque_; -// // // OutputInterface right_drive_belt_control_torque_; - -// // // double drive_belt_working_velocity_; -// // // double sync_coefficient_; -// // // double max_control_torque_; -// // // int drive_belt_block_count_ = 0; - -// // // double launch_trigger_value_; -// // // double launch_lock_value_; -// // // double trigger_free_duration_ms_; -// // // bool trigger_lock_flag_ = false; -// // // OutputInterface trigger_value_; - -// // // rmcs_msgs::DartLaunchStages stage_ = rmcs_msgs::DartLaunchStages::DISABLE; -// // // OutputInterface filling_stage_; - -// // // // Compatibility outputs for CatapultDartV3Full TriggerServo mandatory bindings -// // // uint16_t lifting_up_angle_left_; -// // // uint16_t lifting_up_angle_right_; -// // // uint16_t limiting_lock_angle_; -// // // OutputInterface limiting_control_angle_; -// // // OutputInterface lifting_left_control_angle_; -// // // OutputInterface lifting_right_control_angle_; -// // // }; - -// // // } // namespace rmcs_core::controller::dart - -// // // #include -// // // PLUGINLIB_EXPORT_CLASS( -// // // rmcs_core::controller::dart::DartLaunchControllerV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp index 8a9b5f64..f3fa9d03 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -38,6 +38,9 @@ class DartLaunchSetting register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); + register_output( "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); register_output( @@ -62,17 +65,11 @@ class DartLaunchSetting drive_belt_sync_control(control_velocity); *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; + // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); } private: void drive_belt_sync_control(double set_velocity) { - if (set_velocity == 0.0) { - *left_belt_torque_ = 0.0; - *right_belt_torque_ = 0.0; - belt_pid_.reset(); - return; - } - Eigen::Vector2d setpoint_error{ set_velocity - *left_belt_velocity_, set_velocity - *right_belt_velocity_}; @@ -104,6 +101,8 @@ class DartLaunchSetting InputInterface left_belt_velocity_; InputInterface right_belt_velocity_; + InputInterface force_sensor_ch1_,force_sensor_ch2_; + OutputInterface left_belt_torque_; OutputInterface right_belt_torque_; OutputInterface trigger_value_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp new file mode 100644 index 00000000..7ab4e08b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp @@ -0,0 +1,152 @@ +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::dart { + +// DartLaunchSettingV2 — DartLaunchSetting 的升级版，新增 LK 升降电机速度控制。 +// +// 升降控制逻辑（仅做命令→速度映射，无堵转检测）： +// UP → left = +lifting_velocity, right = -lifting_velocity +// DOWN → left = -lifting_velocity, right = +lifting_velocity +// WAIT → left = right = 0（电机以小电流维持当前位置） +// +// 堵转检测由 LiftingLkAction 内部完成（直接读速度反馈），不在此处处理。 +// +// yaml 新增参数： +// lifting_velocity (double, rad/s) — 升降速度，正值=左+右-（平台上升方向），需实测标定 +class DartLaunchSettingV2 + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartLaunchSettingV2() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , belt_pid_{ + get_parameter("b_kp").as_double(), + get_parameter("b_ki").as_double(), + get_parameter("b_kd").as_double()} { + + belt_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + trigger_free_value_ = get_parameter("trigger_free_value").as_double(); + trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); + lifting_velocity_ = get_parameter("lifting_velocity").as_double(); + belt_hold_torque_ = get_parameter("belt_hold_torque").as_double(); + + register_input("/dart/manager/belt/command", belt_command_); + register_input("/dart/manager/trigger/lock_enable", trigger_lock_enable_); + register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); + register_input("/dart/manager/lifting/command", lifting_command_); + // 第一发下降速度缩放因子（shot_count==1 时 0.5，其余 1.0） + register_input("/dart/manager/belt/down_scale", belt_down_scale_); + + register_output( + "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); + register_output( + "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); + register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); + register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); + register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); + } + + void update() override { + double control_velocity = 0.0; + switch (*belt_command_) { + case rmcs_msgs::DartSliderStatus::DOWN: + control_velocity = +belt_velocity_ * (*belt_down_scale_); + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; + break; + case rmcs_msgs::DartSliderStatus::UP: + control_velocity = -belt_velocity_; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; + break; + default: + control_velocity = 0.0; + break; + } + drive_belt_sync_control(control_velocity); + + *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; + // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); + } + +private: + void drive_belt_sync_control(double set_velocity) { + // WAIT 状态：清除 PID 积分；若上一个方向是 DOWN，施加保持扭矩防止滑落 + if (set_velocity == 0.0) { + belt_pid_.reset(); + if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { + *left_belt_torque_ = +belt_hold_torque_; + *right_belt_torque_ = +belt_hold_torque_; + } else { + *left_belt_torque_ = 0.0; + *right_belt_torque_ = 0.0; + } + return; + } + + Eigen::Vector2d setpoint_error{ + set_velocity - *left_belt_velocity_, + set_velocity - *right_belt_velocity_}; + Eigen::Vector2d relative_velocity{ + *left_belt_velocity_ - *right_belt_velocity_, + *right_belt_velocity_ - *left_belt_velocity_}; + + Eigen::Vector2d control_torques = + belt_pid_.update(setpoint_error) - sync_coefficient_ * relative_velocity; + + *left_belt_torque_ = + std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); + *right_belt_torque_ = + std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + } + + rclcpp::Logger logger_; + + pid::MatrixPidCalculator<2> belt_pid_; + + double belt_velocity_; + double sync_coefficient_; + double max_control_torque_; + double trigger_free_value_; + double trigger_lock_value_; + double lifting_velocity_; + double belt_hold_torque_{0.0}; + + rmcs_msgs::DartSliderStatus prev_belt_cmd_{rmcs_msgs::DartSliderStatus::WAIT}; + + InputInterface belt_command_; + InputInterface trigger_lock_enable_; + InputInterface left_belt_velocity_; + InputInterface right_belt_velocity_; + InputInterface force_sensor_ch1_, force_sensor_ch2_; + + InputInterface lifting_command_; + InputInterface belt_down_scale_; + + OutputInterface left_belt_torque_; + OutputInterface right_belt_torque_; + OutputInterface trigger_value_; + OutputInterface lifting_left_vel_; + OutputInterface lifting_right_vel_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp index 6a4ff3ec..3340531c 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp @@ -1,538 +1,536 @@ -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "filter/low_pass_filter.hpp" -// #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/force_sensor.hpp" -// #include "hardware/device/lk_motor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "hardware/device/trigger_servo.hpp" -// #include "librmcs/agent/c_board.hpp" - -// /* -// CatapultDartV3Lk — catapult_dart_v3_full 的变体 -// 升降电机替换为瓴控4005 (LkMotor MG4005Ei10)，挂 CAN1 (0x141 左, 0x142 右)。 -// 限位舵机保留 TriggerServo (UART2, ID=0x03)。 - -// 升降电机接口 (double, rad): -// 输出: /dart/lifting_left/angle, /dart/lifting_left/velocity 等 -// 输入: /dart/lifting_left/control_angle (由 launch_controller_v2_full 写入) -// */ - -// namespace rmcs_core::hardware { - -// class CatapultDartV3Lk -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::agent::CBoard { -// public: -// CatapultDartV3Lk() -// : Node{get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} -// , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , logger_{get_logger()} -// , pitch_angle_filter_(20.0, 1000.0) -// , pitch_velocity_filter_(20.0, 1000.0) -// , yaw_velocity_filter_(20.0, 1000.0) -// , yaw_filter_(5.0, 1000.0) -// , pitch_filter_(10.0, 1000.0) -// , roll_filter_(10.0, 1000.0) -// , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} -// , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} -// , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} -// , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} -// , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} -// , force_sensor_{*this} -// , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} -// , limiting_servo_{*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID} -// , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} -// , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} -// , dr16_{*this} -// , imu_{1000, 0.2, 0.0} { - -// pitch_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// yaw_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// force_screw_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// drive_belt_motor_left_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// drive_belt_motor_right_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(19.)); - -// // LK4005 lifting motors: multi-turn angle mode for absolute position control -// lifting_left_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} -// .enable_multi_turn_angle()); -// lifting_right_motor_.configure( -// device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} -// .enable_multi_turn_angle()); - -// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); -// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - -// setup_imu_coordinate_mapping(); -// start_time_ = std::chrono::steady_clock::now(); -// yaw_drift_coefficient_ = 0.0; -// tf_broadcaster_ = std::make_unique(*this); - -// register_output("/tf", tf_); -// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); -// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); -// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - -// limiting_calibrate_subscription_ = create_subscription( -// "/limiting/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg)); -// }); - -// register_output("/referee/serial", referee_serial_); -// referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { -// std::lock_guard lock(referee_mutex_); -// size_t count = 0; -// while (count < size && !referee_ring_buffer_receive_.empty()) { -// buffer[count++] = referee_ring_buffer_receive_.front(); -// referee_ring_buffer_receive_.pop_front(); -// } -// return count; -// }; -// referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { -// auto board = start_transmit(); -// board.uart1_transmit({.uart_data = std::span{buffer, size}}); -// return size; -// }; -// } - -// CatapultDartV3Lk(const CatapultDartV3Lk&) = delete; -// CatapultDartV3Lk& operator=(const CatapultDartV3Lk&) = delete; -// CatapultDartV3Lk(CatapultDartV3Lk&&) = delete; -// CatapultDartV3Lk& operator=(CatapultDartV3Lk&&) = delete; -// ~CatapultDartV3Lk() override = default; - -// void update() override { -// dr16_.update_status(); -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_left_.update_status(); -// drive_belt_motor_right_.update_status(); -// force_screw_motor_.update_status(); -// force_sensor_.update_status(); -// lifting_left_motor_.update_status(); -// lifting_right_motor_.update_status(); -// imu_.update_status(); -// processImuData(); -// } - -// void command_update() { -// auto board = start_transmit(); - -// // Trigger servo: PWM via GPIO -// board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - -// // Force sensor: polling command on CAN1 (every 100 cycles) -// if (pub_time_count_++ > 100) { -// board.can1_transmit({ -// .can_id = 0x301, -// .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), -// }); -// pub_time_count_ = 0; -// } - -// // DJI motors on CAN2 (pitch/yaw/force_screw: 0x200, belts: 0x1FF) -// board.can2_transmit({ -// .can_id = 0x200, -// .can_data = device::CanPacket8{ -// pitch_motor_.generate_command(), -// yaw_motor_.generate_command(), -// force_screw_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// }.as_bytes(), -// }); -// board.can2_transmit({ -// .can_id = 0x1FF, -// .can_data = device::CanPacket8{ -// drive_belt_motor_left_.generate_command(), -// drive_belt_motor_right_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// }.as_bytes(), -// }); - -// // LK4005 lifting motors on CAN1 (unicast per motor) -// board.can1_transmit({ -// .can_id = LK_LIFTING_LEFT_ID, -// .can_data = lifting_left_motor_.generate_command().as_bytes(), -// }); -// board.can1_transmit({ -// .can_id = LK_LIFTING_RIGHT_ID, -// .can_data = lifting_right_motor_.generate_command().as_bytes(), -// }); - -// // Limiting servo on UART2 (only send when target changes) -// if (!limiting_servo_.calibrate_mode()) { -// uint16_t current_target = limiting_servo_.get_target_angle(); -// if (current_target != last_limiting_angle_) { -// size_t uart_data_length; -// auto command_buffer = -// limiting_servo_.generate_runtime_command(uart_data_length); -// board.uart2_transmit( -// {.uart_data = std::span{command_buffer.get(), uart_data_length}}); -// last_limiting_angle_ = current_target; -// } -// } -// } - -// protected: -// void can1_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; -// const auto can_id = data.can_id; - -// if (can_id == 0x302) { -// force_sensor_frame_count_++; -// if (force_sensor_frame_count_ % 200 == 1) { -// RCLCPP_INFO( -// logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s", -// data.can_data.size(), -// bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); -// } -// force_sensor_.store_status(data.can_data); -// } else if (can_id == LK_LIFTING_LEFT_ID) { -// lifting_left_motor_.store_status(data.can_data); -// } else if (can_id == LK_LIFTING_RIGHT_ID) { -// lifting_right_motor_.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) [[unlikely]] -// return; -// const auto can_id = data.can_id; - -// if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 -// && can_id != 0x205 && can_id != 0x206) { -// can2_unknown_count_++; -// if (can2_unknown_count_ % 50 == 1) { -// RCLCPP_INFO( -// logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", -// can_id, data.can_data.size(), -// bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); -// } -// } - -// if (can_id == 0x201) { -// pitch_motor_.store_status(data.can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(data.can_data); -// } else if (can_id == 0x203) { -// force_screw_motor_.store_status(data.can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_left_.store_status(data.can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_right_.store_status(data.can_data); -// } -// } - -// void uart1_receive_callback(const librmcs::data::UartDataView& data) override { -// std::lock_guard lock(referee_mutex_); -// for (auto byte : data.uart_data) { -// referee_ring_buffer_receive_.push_back(byte); -// } -// } - -// void uart2_receive_callback(const librmcs::data::UartDataView& data) override { -// const auto* raw_data = data.uart_data.data(); -// const size_t length = data.uart_data.size(); - -// RCLCPP_DEBUG(logger_, "UART2 received: len=%zu [%s]", length, -// bytes_to_hex_string(raw_data, length).c_str()); - -// if (length < 3) { -// RCLCPP_WARN(logger_, "UART2 data too short: %zu bytes", length); -// return; -// } - -// const uint8_t servo_id = static_cast(raw_data[2]); - -// // Only limiting servo remains on UART2 -// if (servo_id == LIMITING_SERVO_ID) { -// auto result = limiting_servo_.calibrate_current_angle(logger_, raw_data, length); -// if (!result.first) -// RCLCPP_INFO(logger_, "calibrate: uart2 limiting data store failed"); -// } else { -// RCLCPP_DEBUG(logger_, "UART2: unknown servo id 0x%02X", servo_id); -// } -// } - -// 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 { -// imu_.store_accelerometer_status(data.x, data.y, data.z); -// } - -// void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { -// imu_.store_gyroscope_status(data.x, data.y, data.z); -// } - -// private: -// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; -// static constexpr uint8_t LIMITING_SERVO_ID = 0x03; -// static constexpr uint32_t LK_LIFTING_LEFT_ID = 0x141; -// static constexpr uint32_t LK_LIFTING_RIGHT_ID = 0x142; - -// void trigger_servo_calibrate_subscription_callback( -// device::TriggerServo& servo, std_msgs::msg::Int32::UniquePtr msg) { -// servo.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; - -// if (msg->data == 0) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = servo.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// if (command_buffer && command_length > 0) { -// auto board = start_transmit(); -// board.uart2_transmit( -// {.uart_data = std::span{command_buffer.get(), command_length}}); -// RCLCPP_INFO(logger_, "UART2 Pub: (length=%zu)[ %s ]", command_length, -// bytes_to_hex_string(command_buffer.get(), command_length).c_str()); -// } -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) -// return "[]"; -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); -// for (size_t i = 0; i < size; ++i) -// ss << std::setw(2) << static_cast(data[i]) << " "; -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') -// result.pop_back(); -// return result; -// } - -// void setup_imu_coordinate_mapping() { -// imu_.set_coordinate_mapping( -// [](double x, double y, double z) -> std::tuple { -// return {x, -y, -z}; -// }); -// } - -// void processImuData() { -// auto current_time = std::chrono::steady_clock::now(); -// double elapsed_seconds = -// std::chrono::duration(current_time - start_time_).count(); - -// Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); -// Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - -// double roll = std::atan2(rot(2, 1), rot(2, 2)); -// double pitch = std::asin(-rot(2, 0)); -// double yaw = std::atan2(rot(1, 0), rot(0, 0)); - -// double t_roll = -roll_filter_.update(roll); -// double t_pitch = pitch_filter_.update(pitch); -// double t_yaw = -yaw_filter_.update(yaw); - -// if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { -// yaw_samples_.push_back(t_yaw); -// time_samples_.push_back(elapsed_seconds); - -// double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; -// double n = static_cast(yaw_samples_.size()); -// for (size_t i = 0; i < yaw_samples_.size(); ++i) { -// sum_x += time_samples_[i]; -// sum_y += yaw_samples_[i]; -// sum_xy += time_samples_[i] * yaw_samples_[i]; -// sum_xx += time_samples_[i] * time_samples_[i]; -// } -// double denominator = n * sum_xx - sum_x * sum_x; -// if (std::abs(denominator) > 1e-10) -// yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; -// } - -// t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; -// publishTfTransforms(t_roll, t_pitch, t_yaw); - -// *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; -// *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; -// *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; -// } - -// void publishTfTransforms(double roll, double pitch, double yaw) { -// auto now = this->get_clock()->now(); - -// auto make_tf = -// [&](const std::string& parent, const std::string& child, -// const geometry_msgs::msg::Vector3& trans, -// const geometry_msgs::msg::Quaternion& rot) { -// geometry_msgs::msg::TransformStamped tf; -// tf.header.stamp = now; -// tf.header.frame_id = parent; -// tf.child_frame_id = child; -// tf.transform.translation = trans; -// tf.transform.rotation = rot; -// return tf; -// }; - -// auto make_vec = [](double x, double y, double z) { -// geometry_msgs::msg::Vector3 v; -// v.x = x; v.y = y; v.z = z; -// return v; -// }; - -// auto make_quat = [](double x, double y, double z, double w) { -// geometry_msgs::msg::Quaternion q; -// q.x = x; q.y = y; q.z = z; q.w = w; -// return q; -// }; - -// Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); -// Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); -// Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); -// Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - -// auto zero = make_vec(0, 0, 0); -// auto p_trans = make_vec(0, 0, 0.05); -// auto id_rot = make_quat(0, 0, 0, 1); - -// tf_broadcaster_->sendTransform( -// make_tf("base_link", "gimbal_center_link", zero, id_rot)); -// tf_broadcaster_->sendTransform(make_tf( -// "gimbal_center_link", "yaw_link", zero, -// make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); -// tf_broadcaster_->sendTransform(make_tf( -// "yaw_link", "pitch_link", p_trans, -// make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); -// tf_broadcaster_->sendTransform(make_tf( -// "pitch_link", "odom_imu", zero, -// make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); -// tf_broadcaster_->sendTransform( -// make_tf("world", "base_link", zero, id_rot)); -// } - -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDartV3Lk& dart) -// : dart_(dart) {} -// void update() override { dart_.command_update(); } - -// private: -// CatapultDartV3Lk& dart_; -// }; - -// std::shared_ptr dart_command_; -// rclcpp::Logger logger_; - -// filter::LowPassFilter<1> pitch_angle_filter_; -// filter::LowPassFilter<1> pitch_velocity_filter_; -// filter::LowPassFilter<1> yaw_velocity_filter_; -// filter::LowPassFilter<1> yaw_filter_; -// filter::LowPassFilter<1> pitch_filter_; -// filter::LowPassFilter<1> roll_filter_; - -// std::chrono::steady_clock::time_point start_time_; - -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_screw_motor_; -// device::DjiMotor drive_belt_motor_left_; -// device::DjiMotor drive_belt_motor_right_; - -// device::ForceSensor force_sensor_; - -// device::PWMServo trigger_servo_; -// device::TriggerServo limiting_servo_; - -// // LK4005 lifting motors (replaces TriggerServo lifting servos) -// device::LkMotor lifting_left_motor_; -// device::LkMotor lifting_right_motor_; - -// device::Dr16 dr16_; -// device::Bmi088 imu_; - -// std::unique_ptr tf_broadcaster_; - -// OutputInterface tf_; -// OutputInterface catapult_pitch_angle_; -// OutputInterface catapult_roll_angle_; -// OutputInterface catapult_yaw_angle_; - -// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; - -// std::mutex referee_mutex_; -// std::deque referee_ring_buffer_receive_; -// OutputInterface referee_serial_; - -// int pub_time_count_ = 0; -// int force_sensor_frame_count_ = 0; -// int can2_unknown_count_ = 0; - -// double first_sample_spot_ = 1.0; -// double final_sample_spot_ = 4.0; -// double yaw_drift_coefficient_ = 0.0; -// std::vector yaw_samples_; -// std::vector time_samples_; - -// uint16_t last_limiting_angle_ = 0xFFFF; -// }; - -// } // namespace rmcs_core::hardware - -// #include -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Lk, rmcs_executor::Component) +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "filter/low_pass_filter.hpp" +#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/force_sensor.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/pwm_servo.hpp" +#include "hardware/device/trigger_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +/* +CatapultDartV3Lk — catapult_dart_v3_full 的变体 +升降电机替换为瓴控4005 (LkMotor MG4005Ei10)，挂 CAN1 (0x141 左, 0x142 右)。 +限位舵机保留 TriggerServo (UART2, ID=0x03)。 + +升降电机接口 (double, rad): + 输出: /dart/lifting_left/angle, /dart/lifting_left/velocity 等 + 输入: /dart/lifting_left/control_angle (由 launch_controller_v2_full 写入) +*/ + +namespace rmcs_core::hardware { + +class CatapultDartV3Lk + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartV3Lk() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , pitch_angle_filter_(20.0, 1000.0) + , pitch_velocity_filter_(20.0, 1000.0) + , yaw_velocity_filter_(20.0, 1000.0) + , yaw_filter_(5.0, 1000.0) + , pitch_filter_(10.0, 1000.0) + , roll_filter_(10.0, 1000.0) + , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} + , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} + , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} + , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} + , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} + , force_sensor_{*this} + , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} + , limiting_servo_{*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID} + , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} + , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} + , dr16_{*this} + , imu_{1000, 0.2, 0.0} { + + pitch_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + yaw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + force_screw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_left_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_right_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + + lifting_left_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + lifting_right_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + + first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); + final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + + setup_imu_coordinate_mapping(); + start_time_ = std::chrono::steady_clock::now(); + yaw_drift_coefficient_ = 0.0; + tf_broadcaster_ = std::make_unique(*this); + + register_output("/tf", tf_); + register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + + limiting_calibrate_subscription_ = create_subscription( + "/limiting/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg)); + }); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { + std::lock_guard lock(referee_mutex_); + size_t count = 0; + while (count < size && !referee_ring_buffer_receive_.empty()) { + buffer[count++] = referee_ring_buffer_receive_.front(); + referee_ring_buffer_receive_.pop_front(); + } + return count; + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { + auto board = start_transmit(); + board.uart1_transmit({.uart_data = std::span{buffer, size}}); + return size; + }; + } + + CatapultDartV3Lk(const CatapultDartV3Lk&) = delete; + CatapultDartV3Lk& operator=(const CatapultDartV3Lk&) = delete; + CatapultDartV3Lk(CatapultDartV3Lk&&) = delete; + CatapultDartV3Lk& operator=(CatapultDartV3Lk&&) = delete; + ~CatapultDartV3Lk() override = default; + + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_left_.update_status(); + drive_belt_motor_right_.update_status(); + force_screw_motor_.update_status(); + force_sensor_.update_status(); + lifting_left_motor_.update_status(); + lifting_right_motor_.update_status(); + imu_.update_status(); + processImuData(); + } + + void command_update() { + auto board = start_transmit(); + + // Trigger servo: PWM via GPIO + board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + + // Force sensor: polling command on CAN1 (every 100 cycles) + if (pub_time_count_++ > 100) { + board.can1_transmit({ + .can_id = 0x301, + .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), + }); + pub_time_count_ = 0; + } + + // DJI motors on CAN2 (pitch/yaw/force_screw: 0x200, belts: 0x1FF) + board.can2_transmit({ + .can_id = 0x200, + .can_data = device::CanPacket8{ + pitch_motor_.generate_command(), + yaw_motor_.generate_command(), + force_screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }); + board.can2_transmit({ + .can_id = 0x1FF, + .can_data = device::CanPacket8{ + drive_belt_motor_left_.generate_command(), + drive_belt_motor_right_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }); + + // LK4005 lifting motors on CAN1 (unicast per motor) + board.can1_transmit({ + .can_id = LK_LIFTING_LEFT_ID, + .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), + }); + board.can1_transmit({ + .can_id = LK_LIFTING_RIGHT_ID, + .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), + }); + + // Limiting servo on UART2 (only send when target changes) + if (!limiting_servo_.calibrate_mode()) { + uint16_t current_target = limiting_servo_.get_target_angle(); + if (current_target != last_limiting_angle_) { + size_t uart_data_length; + auto command_buffer = + limiting_servo_.generate_runtime_command(uart_data_length); + board.uart2_transmit( + {.uart_data = std::span{command_buffer.get(), uart_data_length}}); + last_limiting_angle_ = current_target; + } + } + } + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + const auto can_id = data.can_id; + + if (can_id == 0x302) { + force_sensor_frame_count_++; + if (force_sensor_frame_count_ % 200 == 1) { + RCLCPP_INFO( + logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s", + data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + force_sensor_.store_status(data.can_data); + } else if (can_id == LK_LIFTING_LEFT_ID) { + lifting_left_motor_.store_status(data.can_data); + } else if (can_id == LK_LIFTING_RIGHT_ID) { + lifting_right_motor_.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) [[unlikely]] + return; + const auto can_id = data.can_id; + + if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 + && can_id != 0x205 && can_id != 0x206) { + can2_unknown_count_++; + if (can2_unknown_count_ % 50 == 1) { + RCLCPP_INFO( + logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", + can_id, data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + } + + if (can_id == 0x201) { + pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + force_screw_motor_.store_status(data.can_data); + } else if (can_id == 0x205) { + drive_belt_motor_left_.store_status(data.can_data); + } else if (can_id == 0x206) { + drive_belt_motor_right_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + std::lock_guard lock(referee_mutex_); + for (auto byte : data.uart_data) { + referee_ring_buffer_receive_.push_back(byte); + } + } + + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + const auto* raw_data = data.uart_data.data(); + const size_t length = data.uart_data.size(); + + RCLCPP_DEBUG(logger_, "UART2 received: len=%zu [%s]", length, + bytes_to_hex_string(raw_data, length).c_str()); + + if (length < 3) { + RCLCPP_WARN(logger_, "UART2 data too short: %zu bytes", length); + return; + } + + const uint8_t servo_id = static_cast(raw_data[2]); + + // Only limiting servo remains on UART2 + if (servo_id == LIMITING_SERVO_ID) { + auto result = limiting_servo_.calibrate_current_angle(logger_, raw_data, length); + if (!result.first) + RCLCPP_INFO(logger_, "calibrate: uart2 limiting data store failed"); + } else { + RCLCPP_DEBUG(logger_, "UART2: unknown servo id 0x%02X", servo_id); + } + } + + 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 { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + static constexpr uint8_t LIMITING_SERVO_ID = 0x03; + static constexpr uint32_t LK_LIFTING_LEFT_ID = 0x141; + static constexpr uint32_t LK_LIFTING_RIGHT_ID = 0x145; + + void trigger_servo_calibrate_subscription_callback( + device::TriggerServo& servo, std_msgs::msg::Int32::UniquePtr msg) { + servo.set_calibrate_mode(msg->data); + + std::unique_ptr command_buffer; + size_t command_length = 0; + + if (msg->data == 0) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); + } else if (msg->data == 1) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); + } else if (msg->data == 2) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); + } else if (msg->data == 3) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); + } else if (msg->data == 4) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); + } else if (msg->data == 5) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); + } else if (msg->data == 6) { + command_buffer = servo.generate_calibrate_command( + device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); + } + + if (command_buffer && command_length > 0) { + auto board = start_transmit(); + board.uart2_transmit( + {.uart_data = std::span{command_buffer.get(), command_length}}); + RCLCPP_INFO(logger_, "UART2 Pub: (length=%zu)[ %s ]", command_length, + bytes_to_hex_string(command_buffer.get(), command_length).c_str()); + } + } + + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) + return "[]"; + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + for (size_t i = 0; i < size; ++i) + ss << std::setw(2) << static_cast(data[i]) << " "; + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') + result.pop_back(); + return result; + } + + void setup_imu_coordinate_mapping() { + imu_.set_coordinate_mapping( + [](double x, double y, double z) -> std::tuple { + return {x, -y, -z}; + }); + } + + void processImuData() { + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = + std::chrono::duration(current_time - start_time_).count(); + + Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); + + double roll = std::atan2(rot(2, 1), rot(2, 2)); + double pitch = std::asin(-rot(2, 0)); + double yaw = std::atan2(rot(1, 0), rot(0, 0)); + + double t_roll = -roll_filter_.update(roll); + double t_pitch = pitch_filter_.update(pitch); + double t_yaw = -yaw_filter_.update(yaw); + + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + yaw_samples_.push_back(t_yaw); + time_samples_.push_back(elapsed_seconds); + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + double n = static_cast(yaw_samples_.size()); + for (size_t i = 0; i < yaw_samples_.size(); ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) + yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; + } + + t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + publishTfTransforms(t_roll, t_pitch, t_yaw); + + *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; + *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; + *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; + } + + void publishTfTransforms(double roll, double pitch, double yaw) { + auto now = this->get_clock()->now(); + + auto make_tf = + [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; + + auto make_vec = [](double x, double y, double z) { + geometry_msgs::msg::Vector3 v; + v.x = x; v.y = y; v.z = z; + return v; + }; + + auto make_quat = [](double x, double y, double z, double w) { + geometry_msgs::msg::Quaternion q; + q.x = x; q.y = y; q.z = z; q.w = w; + return q; + }; + + Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; + + auto zero = make_vec(0, 0, 0); + auto p_trans = make_vec(0, 0, 0.05); + auto id_rot = make_quat(0, 0, 0, 1); + + tf_broadcaster_->sendTransform( + make_tf("base_link", "gimbal_center_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf( + "gimbal_center_link", "yaw_link", zero, + make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "yaw_link", "pitch_link", p_trans, + make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "pitch_link", "odom_imu", zero, + make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); + tf_broadcaster_->sendTransform( + make_tf("world", "base_link", zero, id_rot)); + } + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartV3Lk& dart) + : dart_(dart) {} + void update() override { dart_.command_update(); } + + private: + CatapultDartV3Lk& dart_; + }; + + std::shared_ptr dart_command_; + rclcpp::Logger logger_; + + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + filter::LowPassFilter<1> yaw_filter_; + filter::LowPassFilter<1> pitch_filter_; + filter::LowPassFilter<1> roll_filter_; + + std::chrono::steady_clock::time_point start_time_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor force_screw_motor_; + device::DjiMotor drive_belt_motor_left_; + device::DjiMotor drive_belt_motor_right_; + + device::ForceSensor force_sensor_; + + device::PWMServo trigger_servo_; + device::TriggerServo limiting_servo_; + + // LK4005 lifting motors (replaces TriggerServo lifting servos) + device::LkMotor lifting_left_motor_; + device::LkMotor lifting_right_motor_; + + device::Dr16 dr16_; + device::Bmi088 imu_; + + std::unique_ptr tf_broadcaster_; + + OutputInterface tf_; + OutputInterface catapult_pitch_angle_; + OutputInterface catapult_roll_angle_; + OutputInterface catapult_yaw_angle_; + + rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; + + std::mutex referee_mutex_; + std::deque referee_ring_buffer_receive_; + OutputInterface referee_serial_; + + int pub_time_count_ = 0; + int force_sensor_frame_count_ = 0; + int can2_unknown_count_ = 0; + + double first_sample_spot_ = 1.0; + double final_sample_spot_ = 4.0; + double yaw_drift_coefficient_ = 0.0; + std::vector yaw_samples_; + std::vector time_samples_; + + uint16_t last_limiting_angle_ = 0xFFFF; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Lk, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp index 1d981f97..b0ba0d2d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp @@ -26,6 +26,7 @@ #include "hardware/device/bmi088.hpp" #include "hardware/device/can_packet.hpp" #include "hardware/device/dji_motor.hpp" +#include "hardware/device/lk_motor.hpp" #include "hardware/device/dr16.hpp" #include "hardware/device/force_sensor.hpp" #include "hardware/device/pwm_servo.hpp" @@ -56,6 +57,8 @@ class CatapultDartV4 , force_screw_motor_{*this, *dart_command_, "/dart/force_screw"} , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} + , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} + , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} , force_sensor_{*this} , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} @@ -88,6 +91,13 @@ class CatapultDartV4 register_output("/imu/catapult_roll_angle", catapult_roll_angle_); register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + lifting_left_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + lifting_right_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + force_sensor_calibrate_subscription_ = create_subscription( "/force_sensor/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { @@ -153,6 +163,16 @@ class CatapultDartV4 pub_time_count_ = 0; } + board.can1_transmit({ + .can_id = 0x141, + .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), + }); + + board.can1_transmit({ + .can_id = 0x145, + .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), + }); + board.can2_transmit({ .can_id = 0x200, .can_data = @@ -390,6 +410,9 @@ class CatapultDartV4 device::DjiMotor drive_belt_motor_left_; device::DjiMotor drive_belt_motor_right_; + device::LkMotor lifting_left_motor_; + device::LkMotor lifting_right_motor_; + device::ForceSensor force_sensor_; device::PWMServo trigger_servo_; diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index 9f0a7be5..d09af3dd 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit 9f0a7be53926d2451ccc7c70567bf294699fec38 +Subproject commit d09af3dd7cebf2b1af4067a938f1b89d1824834a From 5a2295f1984f06aed40a5ce5a921ac10e40bb4c2 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Sun, 15 Mar 2026 22:53:41 +0800 Subject: [PATCH 38/45] developing special handling for 2-4 launches.v1 means handling without filling --- .../config/dart-filling-test.yaml | 24 - .../rmcs_bringup/config/dart-launch-full.yaml | 74 -- .../config/dart-remot-bridge-test.yaml | 16 - rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml | 14 - .../src/controller/dart/dart_filling_test.cpp | 206 ---- .../src/controller/dart/debug_info.cpp | 272 ----- .../dart/launch_controller_v2_full.cpp | 359 ------ .../src/controller/dart/pwm_test.cpp | 28 - .../src/controller/dart/servo_test.cpp | 97 -- .../dartlauncher/dart_setttings.cpp | 109 -- .../dartlauncher/launch_controller.cpp | 209 ---- .../rmcs_core/src/hardware/catapult_dart.cpp | 1016 ++++++++--------- .../src/hardware/catapult_dart_librmcs_v3.cpp | 142 --- ...lt_dart_v3_lk.cpp => catapult_dart_v2.cpp} | 0 .../src/hardware/catapult_dart_v3_full.cpp | 426 ------- .../src/hardware/catapult_dart_v4.cpp | 449 -------- .../hardware/dart_filling_test_hardware.cpp | 337 ------ rmcs_ws/src/rmcs_dart_guidance | 2 +- 18 files changed, 450 insertions(+), 3330 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp rename rmcs_ws/src/rmcs_core/src/hardware/{catapult_dart_v3_lk.cpp => catapult_dart_v2.cpp} (100%) delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml deleted file mode 100644 index 17f481b7..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml +++ /dev/null @@ -1,24 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Lk -> catapult_dart - - rmcs_core::controller::dart::DartFillingTest -> launch_controller - -catapult_dart: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -launch_controller: - ros__parameters: - # Trigger servo: PWM value 0.0–1.0 - # PWMServo range: 0.5ms–2.5ms in a 20ms period - # 0.0 → 0.5ms (full CCW), 0.5 → 1.5ms (center), 1.0 → 2.5ms (full CW) - trigger_free_value: 0.75 # tune to match physical free position - trigger_lock_value: 0.63 # tune to match physical lock position - - # Motor torque limits (N·m) - max_torque: 1.0 - max_belt_torque: 5.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml deleted file mode 100644 index bf3959f6..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml +++ /dev/null @@ -1,74 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware - - rmcs_core::controller::dart::DartLaunchControllerV2Full -> launch_controller - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster - -broadcaster: - ros__parameters: - forward_list: - - /dart/drive_belt/left/control_torque - - /dart/drive_belt/left/velocity - - /dart/drive_belt/right/control_torque - - /dart/drive_belt/right/velocity - - /dart/lifting_left/angle - - /dart/lifting_right/angle - - /force/sensor/average - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -launch_controller: - ros__parameters: - belt_velocity: 10.0 - sync_coefficient: 0.1 - max_control_torque: 15.0 - # Trigger servo PWM value 0.0–1.0 (from dart-filling-test calibration) - trigger_free_value: 0.75 - trigger_lock_value: 0.66 - # Duration to keep trigger free after launch (ms) - trigger_free_duration_ms: 300 - # Time to wait after platform move before belt resets (ms) - lifting_settle_ms: 500 - # Duration to keep limiting servo open for dart feeding (ms) - limiting_open_duration_ms: 2000 - # Lifting motor angles in DEGREES — will be converted to radians in code. - # 待标定: 上电后将平台移到 up 位置，记录角度填入 *_up_deg； - # 移到 down 位置记录填入 *_down_deg。 - lifting_up_angle_left_deg: 0.0 - lifting_down_angle_left_deg: 90.0 - lifting_up_angle_right_deg: 0.0 - lifting_down_angle_right_deg: 90.0 - # Limiting servo (TriggerServo UART, uint16_t hex) - limiting_free_angle_: 0x0001 - limiting_lock_angle_: 0x0050 - -dart_settings: - ros__parameters: - is_auto_pitch_control_mode: 0 - is_auto_force_control_mode: 0 - max_transform_rate: 5.0 - pitch_angle_kp: 1.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - force_control_kp: 1.0 - force_control_ki: 0.0 - force_control_kd: 0.0 - -force_screw_velocity_pid_controller: - ros__parameters: - measurement: /dart/force_screw_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_screw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml deleted file mode 100644 index d0ed8157..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml +++ /dev/null @@ -1,16 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge - - rmcs_core::hardware::CatapultDartV4 -> dart_hardware - -remote_cmd_bridge: - ros__parameters: - joystick_sensitivity: 0.01 - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml b/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml deleted file mode 100644 index 2a1c3b81..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml +++ /dev/null @@ -1,14 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartforlibrmcsv3 -> catapult_dart_hardware - - rmcs_core::controller::test::PWMTest -> pwm_test_controller - -catapult_dart_hardware: - ros__parameters: - serial_filter: "" - -pwm_test_controller: - ros__parameters: - set_value: 0.5 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp deleted file mode 100644 index 8a0d9ac8..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp +++ /dev/null @@ -1,206 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -DartFillingTest — 新方案综合验证控制器 -键位： -双下 / UNKNOWN：全部停止 - -左拨杆中：调整角度模式 - 左摇杆 Y → yaw 电机力矩 - 右摇杆 X → pitch 电机力矩 - 右摇杆 Y → 传送带电机力矩（左右同步） - 右拨杆下拨再回中：降下抬升平台 - 右拨杆上拨再回中：抬起抬升平台 - -左拨杆上：发射/弹仓控制模式 - 右摇杆 X → 螺旋推力电机力矩 - 右拨杆下拨再回中：锁定扳机 + 限位舵机锁定 - 右拨杆上拨再回中：释放扳机 + 限位舵机释放 -*/ - -namespace rmcs_core::controller::dart { - -class DartFillingTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartFillingTest() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , timer_interval_ms_(10) - , logger_(get_logger()) { - - // Trigger servo: PWM value (double 0.0–1.0) - trigger_free_value_ = get_parameter("trigger_free_value").as_double(); - trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); - - max_torque_ = get_parameter("max_torque").as_double(); - max_belt_torque_ = get_parameter("max_belt_torque").as_double(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - - register_input("/dart/lifting_left/angle", lifting_left_angle_); - register_input("/dart/lifting_right/angle", lifting_right_angle_); - - // Trigger servo: PWM double value - register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); - - // Motor torque outputs (direct control, no PID needed in test) - register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); - register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); - register_output("/dart/force_screw_motor/control_torque", force_screw_torque_, 0.0); - register_output("/dart/drive_belt/left/control_torque", belt_left_torque_, 0.0); - register_output("/dart/drive_belt/right/control_torque", belt_right_torque_, 0.0); - - register_output("/dart/lifting_left/control_velocity", lifting_left_angle_shift_); - register_output("/dart/lifting_right/control_velocity", lifting_right_angle_shift_); - register_output("/dart/filling/stage", filling_stage_); - register_output("/force/sensor/average", average_force_); - register_output("/dart/limiting_servo/control_angle", limiting_control_angle_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms_), - [this] { timer_callback(); }); - } - - void update() override { - using namespace rmcs_msgs; - - auto sw_left = *switch_left_; - auto sw_right = *switch_right_; - - const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) - && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); - - if (all_down) { - disable_all(); - } else if (sw_left == Switch::MIDDLE) { - // Angle / belt control mode - *yaw_torque_ = std::clamp(joystick_left_->y() * max_torque_, - -max_torque_, max_torque_); - *pitch_torque_ = std::clamp(joystick_right_->x() * max_torque_, - -max_torque_, max_torque_); - - double lifting_cmd = joystick_right_->y() * max_belt_torque_; - *lifting_left_angle_shift_ = lifting_cmd; - *lifting_right_angle_shift_ = -lifting_cmd; - - *force_screw_torque_ = 0.0; - - } else if (sw_left == Switch::UP) { - // Force-screw / trigger control mode - *force_screw_torque_ = std::clamp(joystick_right_->x() * max_torque_, - -max_torque_, max_torque_); - *yaw_torque_ = 0.0; - *pitch_torque_ = 0.0; - *belt_left_torque_ = 0.0; - *belt_right_torque_ = 0.0; - - // Right switch transitions: trigger / limiting servo - if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { - *trigger_value_ = trigger_lock_value_; - } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { - *trigger_value_ = trigger_free_value_; - } - } - - *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; - log_count_++; - if (log_count_ >= 1000) { - log_count_ = 0; - RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", - *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); - RCLCPP_INFO(logger_, "[Lifting] left: %.4f rad right: %.4f rad", - *lifting_left_angle_, *lifting_right_angle_); - } - - *limiting_control_angle_ = 0; - last_switch_left_ = sw_left; - last_switch_right_ = sw_right; - } - -private: - void disable_all() { - *trigger_value_ = trigger_lock_value_; - *yaw_torque_ = 0.0; - *pitch_torque_ = 0.0; - *force_screw_torque_ = 0.0; - *belt_left_torque_ = 0.0; - *belt_right_torque_ = 0.0; - } - - rclcpp::TimerBase::SharedPtr timer_; - int timer_interval_ms_; - std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; - - void timer_callback() { - if (is_delaying_ && delay_remaining_ms_ > 0) { - delay_remaining_ms_ -= timer_interval_ms_; - if (delay_remaining_ms_ <= 0) { - is_delaying_ = false; - if (delayed_action_) - delayed_action_(); - } - } - } - - int log_count_ = 0; - - rclcpp::Logger logger_; - - // Remote inputs - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - - // Servo outputs - double trigger_free_value_; - double trigger_lock_value_; - OutputInterface trigger_value_; - - // Motor torque outputs (direct) - double max_torque_; - double max_belt_torque_; - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; - OutputInterface yaw_torque_; - OutputInterface pitch_torque_; - OutputInterface force_screw_torque_; - OutputInterface belt_left_torque_; - OutputInterface belt_right_torque_; - OutputInterface lifting_left_angle_shift_; - OutputInterface lifting_right_angle_shift_; - - // LK lifting motor angle feedback (radians, multi-turn) - InputInterface lifting_left_angle_; - InputInterface lifting_right_angle_; - OutputInterface limiting_control_angle_; - OutputInterface average_force_; - - // Stage output (used by broadcaster) - OutputInterface filling_stage_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFillingTest, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp deleted file mode 100644 index 098d1542..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp +++ /dev/null @@ -1,272 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::controller::dart { -class Test - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Test() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - register_input("/dart/pitch_motor/control_torque", pitch_control_torque_); - register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); - register_input("/dart/pitch_motor/velocity", pitch_speed_); - register_input("/dart/yaw_motor/velocity", yaw_speed_); - register_input("/imu/catapult_roll_angle", final_roll_); - register_input("/imu/catapult_pitch_angle", final_pitch_); - register_input("/imu/catapult_yaw_angle", final_yaw_); - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - register_input("/dart/lifting_left/current_angle", lifting_angle_left_); - register_input("/dart/lifting_right/current_angle", lifting_angle_right_); - register_input("/dart/lifting_left/control_angle", lifting_control_angle_left_); - register_input("/dart/lifting_right/control_angle", lifting_control_angle_right_); - - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - - declare_parameter("fit_window_size", 50); - declare_parameter("fit_threshold", 1.0); - declare_parameter("stable_min_points", 20); - declare_parameter("stable_region_timeout", 500); - - fit_window_size_ = static_cast(get_parameter("fit_window_size").as_int()); - fit_threshold_ = get_parameter("fit_threshold").as_double(); - stable_min_points_ = static_cast(get_parameter("stable_min_points").as_int()); - stable_region_timeout_ = static_cast(get_parameter("stable_region_timeout").as_int()); - - total_query_pairs_ = 0; - sync_query_pairs_ = 0; - left_has_pending_ = false; - right_has_pending_ = false; - left_pending_delta_ = 0; - right_pending_delta_ = 0; - } - - void update() override { - // if (count_n % 100 == 0) { - // RCLCPP_INFO(logger_, "left_force: %d, right_force: %d", - // *force_sensor_ch1_data_, *force_sensor_ch2_data_); - // } - // count_n++; - if (*lifting_angle_left_ != last_left_angle_for_sync_) { - int16_t delta = calculateAngleDelta(last_left_angle_for_sync_, *lifting_angle_left_); - last_left_angle_for_sync_ = *lifting_angle_left_; - left_has_pending_ = true; - left_pending_delta_ = delta; - } - - if (*lifting_angle_right_ != last_right_angle_for_sync_) { - int16_t delta = calculateAngleDelta(last_right_angle_for_sync_, *lifting_angle_right_); - last_right_angle_for_sync_ = *lifting_angle_right_; - right_has_pending_ = true; - right_pending_delta_ = delta; - } - - if (left_has_pending_ && right_has_pending_) { - total_query_pairs_++; - int16_t delta_diff = std::abs(left_pending_delta_ - right_pending_delta_); - if (delta_diff <= 3) { - sync_query_pairs_++; - } else { - RCLCPP_WARN(logger_, "Lift sync mismatch: left delta=%d, right delta=%d, diff=%d", - left_pending_delta_, right_pending_delta_, delta_diff); - } - left_has_pending_ = false; - right_has_pending_ = false; - } - - recordAngleToWindow(); - - checkStableRegion(); - - if (is_in_stable_region_) { - calculateVibrationInStableRegion(); - } - - if (count_n % 1000 == 0) { - syn = (count_n > 1) ? (double)sync_coefficient_count_ / (count_n - 1) : 0.0; - double sync_ratio = (total_query_pairs_ > 0) ? (double)sync_query_pairs_ / total_query_pairs_ : 0.0; - - if (total_stable_points_ > 0) { - vib = (double)stable_at_control_angle_count_ / total_stable_points_; - RCLCPP_INFO(logger_, - "left_angle=%d, right_angle=%d | " - "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=%.4f", - *lifting_angle_left_, *lifting_angle_right_, - syn, sync_ratio, sync_query_pairs_, total_query_pairs_, vib); - } else { - RCLCPP_INFO(logger_, - "left_angle=%d, right_angle=%d | " - "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=N/A", - *lifting_angle_left_, *lifting_angle_right_, - syn, sync_ratio, sync_query_pairs_, total_query_pairs_); - } - } - - count_n++; - } - -private: - static int16_t calculateAngleDelta(uint16_t last_angle, uint16_t current_angle) { - int16_t delta = static_cast(current_angle - last_angle); - - if (delta > 32767) { - delta -= static_cast(65536); - } else if (delta < -32768) { - delta += static_cast(65536); - } - - return delta; - } - - void recordAngleToWindow() { - angle_window_.push_back(*lifting_angle_left_); - if (angle_window_.size() > fit_window_size_) { - angle_window_.pop_front(); - } - } - - void checkStableRegion() { - if (angle_window_.size() < stable_min_points_) { - stable_region_timeout_counter_ = 0; - return; - } - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - size_t n = angle_window_.size(); - - for (size_t i = 0; i < n; ++i) { - double x = static_cast(i); - double y = static_cast(angle_window_[i]); - sum_x += x; - sum_y += y; - sum_xy += x * y; - sum_xx += x * x; - } - - double b = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x); - double a = (sum_y - b * sum_x) / n; - - double residual_sum = 0.0; - for (size_t i = 0; i < n; ++i) { - double x = static_cast(i); - double y = static_cast(angle_window_[i]); - double y_fit = a + b * x; - residual_sum += (y - y_fit) * (y - y_fit); - } - - double rms_error = sqrt(residual_sum / n); - - bool is_stable = (std::abs(b) < 0.05 && rms_error < fit_threshold_); - - if (is_stable) { - if (!is_in_stable_region_) { - is_in_stable_region_ = true; - current_stable_start_point_ = angle_window_[angle_window_.size() - 1]; - stable_region_timeout_counter_ = 0; - - // RCLCPP_DEBUG(logger_, "Enter stable region: slope=%.4f, rms=%.4f", b, rms_error); - } - } else { - if (is_in_stable_region_) { - stable_region_timeout_counter_++; - if (stable_region_timeout_counter_ > stable_region_timeout_) { - is_in_stable_region_ = false; - stable_region_timeout_counter_ = 0; - - if (current_stable_total_points_ > 0) { - double stable_ratio = static_cast(current_stable_at_control_angle_) / - current_stable_total_points_; - // RCLCPP_DEBUG(logger_, "Exit stable region: total=%d, at control=%d, ratio=%.4f", - // current_stable_total_points_, current_stable_at_control_angle_, stable_ratio); - } - - current_stable_total_points_ = 0; - current_stable_at_control_angle_ = 0; - } - } - } - } - - void calculateVibrationInStableRegion() { - current_stable_total_points_++; - if (*lifting_angle_left_ == *lifting_control_angle_left_ && - *lifting_angle_right_ == *lifting_control_angle_right_) { - current_stable_at_control_angle_++; - } - - total_stable_points_++; - if (*lifting_angle_left_ == *lifting_control_angle_left_ && - *lifting_angle_right_ == *lifting_control_angle_right_) { - stable_at_control_angle_count_++; - } - } - -private: - int count_n = 1; - int sync_coefficient_count_ = 0; - rclcpp::Logger logger_; - double syn = 0.0; - double vib = 0.0; - - uint16_t last_left_angle_for_sync_ = 0; - uint16_t last_right_angle_for_sync_ = 0; - bool left_has_pending_; - bool right_has_pending_; - int16_t left_pending_delta_; - int16_t right_pending_delta_; - int64_t total_query_pairs_; - int64_t sync_query_pairs_; - - std::deque angle_window_; - - bool is_in_stable_region_ = false; - int stable_region_timeout_counter_ = 0; - uint16_t current_stable_start_point_ = 0; - - int current_stable_total_points_ = 0; - int current_stable_at_control_angle_ = 0; - - int total_stable_points_ = 0; - int stable_at_control_angle_count_ = 0; - - int fit_window_size_ = 50; - double fit_threshold_ = 1.0; - int stable_min_points_ = 20; - int stable_region_timeout_ = 500; - - InputInterface pitch_control_torque_; - InputInterface yaw_control_torque_; - InputInterface pitch_speed_; - InputInterface yaw_speed_; - InputInterface final_pitch_; - InputInterface final_roll_; - InputInterface final_yaw_; - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - InputInterface lifting_control_angle_left_; - InputInterface lifting_control_angle_right_; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_middle_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_middle_angle_right_; - uint16_t lifting_down_angle_right_; -}; -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::Test, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp deleted file mode 100644 index 490c5f9f..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp +++ /dev/null @@ -1,359 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -DartLaunchControllerV2Full — 发射控制 Phase 2（含升降电机与限位舵机） -升降电机：瓴控4005 (LkMotor)，control_angle 接口为 double (rad)。 - yaml 参数用度数，代码内自动换算弧度。 -限位舵机：TriggerServo UART，control_angle 接口为 uint16_t。 - -完整发射流程状态机： - DISABLE → 左中 → RESETTING - RESETTING（带反转）→ 堵转 → INIT（带停，trigger LOCK） - INIT + 右中→下 → LOADING（带正转） - LOADING（带正转）→ 堵转 → 降下升降平台 + 延时500ms - → RESETTING（trigger LOCK）+ loading_process（限位舵机开→延时2s→关） - RESETTING（trigger LOCK）→ 堵转 → READY（带停，trigger LOCK） - READY + 右中→上 → 发射：trigger FREE + 升起升降平台 - → 延时后 trigger LOCK → RESETTING（下一轮） - READY + 右中→下 → CANCEL：升起升降平台 → RESETTING（trigger LOCK=false） -*/ - -namespace rmcs_core::controller::dart { - -class DartLaunchControllerV2Full - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchControllerV2Full() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , timer_interval_ms_(10) - , logger_(get_logger()) { - - drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); - launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); - trigger_free_duration_ms_ = get_parameter("trigger_free_duration_ms").as_int(); - lifting_settle_ms_ = get_parameter("lifting_settle_ms").as_int(); - limiting_open_duration_ms_ = get_parameter("limiting_open_duration_ms").as_int(); - - // Lifting angles: yaml in degrees, stored as radians - constexpr double kDegToRad = std::numbers::pi / 180.0; - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left_deg").as_double() - * kDegToRad; - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left_deg").as_double() - * kDegToRad; - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right_deg").as_double() - * kDegToRad; - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right_deg").as_double() - * kDegToRad; - - // Limiting servo angles (UART TriggerServo, uint16_t hex values) - limiting_free_angle_ = static_cast( - get_parameter("limiting_free_angle_").as_int()); - limiting_lock_angle_ = static_cast( - get_parameter("limiting_lock_angle_").as_int()); - - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - - // Lifting angle feedback from LkMotor (double, rad) - register_input("/dart/lifting_left/angle", lifting_angle_left_); - register_input("/dart/lifting_right/angle", lifting_angle_right_); - - register_output( - "/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0.0); - register_output( - "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0.0); - register_output("/dart/trigger_servo/value", trigger_value_, launch_lock_value_); - - // Lifting control: double (rad) → LkMotor control_angle - // Start with NaN to disable motors until explicitly commanded - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle_, - static_cast(std::numeric_limits::quiet_NaN())); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle_, - static_cast(std::numeric_limits::quiet_NaN())); - - // Limiting servo: uint16_t → TriggerServo control_angle, start locked - register_output("/dart/limiting_servo/control_angle", limiting_control_angle_, - limiting_lock_angle_); - - register_output("/dart/filling/stage", filling_stage_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms_), - [this] { timer_callback(); }); - } - - void update() override { - using namespace rmcs_msgs; - - auto sw_left = *switch_left_; - auto sw_right = *switch_right_; - - const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) - && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); - - if (all_down) { - reset_all_controls(); - // SAFETY: trigger LOCK immediately, no dependency on flag - *trigger_value_ = launch_lock_value_; - - } else if (sw_left == Switch::MIDDLE) { - - // Transition out of DISABLE - if (stage_ == DartLaunchStages::DISABLE) { - stage_ = DartLaunchStages::RESETTING; - drive_belt_block_count_ = 0; - // Raise platform at startup - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - *filling_stage_ = DartFillingStages::LIFTING; - RCLCPP_INFO(logger_, "DISABLE → RESETTING, lifting UP"); - } - - // Switch edge: right MIDDLE → DOWN - if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { - if (stage_ == DartLaunchStages::INIT) { - stage_ = DartLaunchStages::LOADING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "INIT → LOADING"); - } else if (stage_ == DartLaunchStages::READY) { - // Cancel: lift platform back up, keep trigger LOCKED during CANCEL/RESETTING - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - *filling_stage_ = DartFillingStages::LIFTING; - stage_ = DartLaunchStages::CANCEL; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "READY → CANCEL, lifting UP, trigger stays LOCKED"); - } - - // Switch edge: right MIDDLE → UP - } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { - if (stage_ == DartLaunchStages::READY) { - // Launch! - trigger_lock_flag_ = false; - stage_ = DartLaunchStages::INIT; // stop belt during fire - *filling_stage_ = DartFillingStages::INIT; - // Raise platform simultaneously - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - RCLCPP_INFO(logger_, "READY → LAUNCH (trigger free, lifting UP)"); - delay_and_execute(trigger_free_duration_ms_, [this]() { - trigger_lock_flag_ = true; - stage_ = DartLaunchStages::RESETTING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "LAUNCH → RESETTING"); - }); - } else { - RCLCPP_WARN(logger_, "Cannot launch: not in READY state (current: %d)", - static_cast(stage_)); - } - } - - // Blocking detection → automatic stage transitions - if (blocking_detection()) { - if (stage_ == DartLaunchStages::RESETTING) { - // trigger_lock_flag_ determines next state - if (trigger_lock_flag_) { - stage_ = DartLaunchStages::READY; - *filling_stage_ = DartFillingStages::READY; - RCLCPP_INFO(logger_, "RESETTING → READY (blocked, lock=true)"); - } else { - stage_ = DartLaunchStages::INIT; - *filling_stage_ = DartFillingStages::INIT; - RCLCPP_INFO(logger_, "RESETTING → INIT (blocked, lock=false)"); - } - drive_belt_block_count_ = 0; - - } else if (stage_ == DartLaunchStages::LOADING) { - // Lower the lifting platform, then start resetting - *lifting_left_control_angle_ = lifting_down_angle_left_; - *lifting_right_control_angle_ = lifting_down_angle_right_; - *filling_stage_ = DartFillingStages::DOWNING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "LOADING blocked → lifting DOWN"); - delay_and_execute(lifting_settle_ms_, [this]() { - stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = true; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "Lifting settled → RESETTING (lock=true)"); - // Open limiting servo to let dart feed, then close - loading_process(); - }); - - } else if (stage_ == DartLaunchStages::CANCEL) { - // Platform fully raised; RESETTING will decompress spring. - // trigger_lock_flag_ = false so RESETTING → INIT (not READY) - stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = false; - drive_belt_block_count_ = 0; - *filling_stage_ = DartFillingStages::INIT; - RCLCPP_INFO(logger_, "CANCEL → RESETTING (lock=false → INIT)"); - } - } - - // Belt velocity based on current stage - double control_velocity = 0.0; - if (stage_ == DartLaunchStages::RESETTING) { - control_velocity = -drive_belt_working_velocity_; - } else if (stage_ == DartLaunchStages::LOADING - || stage_ == DartLaunchStages::CANCEL) { - control_velocity = drive_belt_working_velocity_; - } - drive_belt_sync_control(control_velocity); - - // Trigger servo: updated only in MIDDLE mode - *trigger_value_ = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; - } - // Note: in LEFT_UP (DartSettings) mode, trigger maintains its last value - - last_switch_left_ = sw_left; - last_switch_right_ = sw_right; - } - -private: - void reset_all_controls() { - stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - *left_drive_belt_control_torque_ = 0.0; - *right_drive_belt_control_torque_ = 0.0; - // Cancel any pending delayed action (e.g. mid-launch or mid-loading timer) - is_delaying_ = false; - delayed_action_ = nullptr; - trigger_lock_flag_ = false; - drive_belt_block_count_ = 0; - *limiting_control_angle_ = limiting_lock_angle_; - // Trigger LOCK is set directly in the all_down branch of update() - } - - void loading_process() { - // Open limiting servo to let dart drop into position - *limiting_control_angle_ = limiting_free_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; - RCLCPP_INFO(logger_, "loading_process: limiting servo OPEN"); - delay_and_execute(limiting_open_duration_ms_, [this]() { - *limiting_control_angle_ = limiting_lock_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::READY; - RCLCPP_INFO(logger_, "loading_process: limiting servo LOCK"); - }); - } - - void drive_belt_sync_control(double set_velocity) { - if (set_velocity == 0.0) { - *left_drive_belt_control_torque_ = 0.0; - *right_drive_belt_control_torque_ = 0.0; - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_drive_belt_velocity_, - set_velocity - *right_drive_belt_velocity_}; - Eigen::Vector2d relative_velocity{ - *left_drive_belt_velocity_ - *right_drive_belt_velocity_, - *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - - *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); - } - - bool blocking_detection() { - if ((std::abs(*left_drive_belt_velocity_) < 0.5 - && std::abs(*left_drive_belt_control_torque_) > 0.5) - || (std::abs(*right_drive_belt_velocity_) < 0.5 - && std::abs(*right_drive_belt_control_torque_) > 0.5)) { - drive_belt_block_count_++; - } - return drive_belt_block_count_ > 1000; - } - - rclcpp::TimerBase::SharedPtr timer_; - int timer_interval_ms_; - std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; - - void timer_callback() { - if (is_delaying_ && delay_remaining_ms_ > 0) { - delay_remaining_ms_ -= timer_interval_ms_; - if (delay_remaining_ms_ <= 0) { - is_delaying_ = false; - if (delayed_action_) - delayed_action_(); - } - } - } - - void delay_and_execute(int delay_ms, std::function action) { - if (!is_delaying_) { - is_delaying_ = true; - delay_remaining_ms_ = delay_ms; - delayed_action_ = std::move(action); - } - } - - rclcpp::Logger logger_; - - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - - InputInterface left_drive_belt_velocity_; - InputInterface right_drive_belt_velocity_; - OutputInterface left_drive_belt_control_torque_; - OutputInterface right_drive_belt_control_torque_; - - double drive_belt_working_velocity_; - double sync_coefficient_; - double max_control_torque_; - int drive_belt_block_count_ = 0; - - double launch_trigger_value_; - double launch_lock_value_; - int trigger_free_duration_ms_; - bool trigger_lock_flag_ = false; - OutputInterface trigger_value_; - - // Lifting motors (LK4005): double angle in radians - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - OutputInterface lifting_left_control_angle_; - OutputInterface lifting_right_control_angle_; - double lifting_up_angle_left_; - double lifting_down_angle_left_; - double lifting_up_angle_right_; - double lifting_down_angle_right_; - int lifting_settle_ms_; - - // Limiting servo (TriggerServo UART): uint16_t - OutputInterface limiting_control_angle_; - uint16_t limiting_free_angle_; - uint16_t limiting_lock_angle_; - int limiting_open_duration_ms_; - - rmcs_msgs::DartLaunchStages stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - OutputInterface filling_stage_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchControllerV2Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp deleted file mode 100644 index 5b6ffea5..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp +++ /dev/null @@ -1,28 +0,0 @@ - -#include -#include - -namespace rmcs_core::controller::test { -class PWMTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - PWMTest() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - set_value_ = get_parameter("set_value").as_double(); - register_output("/dart/trigger_servo/value", value_); - } - - void update() override { *value_ = set_value_; } - -private: - double set_value_ = 0.0; - OutputInterface value_; -}; -} // namespace rmcs_core::controller::test - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::test::PWMTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp deleted file mode 100644 index e89f6707..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -namespace rmcs_core::hardware { -class ServoTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - ServoTest() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - trigger_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - trigger_free_angle_ = get_parameter("trigger_free_angle").as_int(); - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_middle_angle_left_ = get_parameter("lifting_middle_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_middle_angle_right_ = get_parameter("lifting_middle_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); - - } - - void update() override { - using namespace rmcs_msgs; - - *trigger_control_angle = trigger_lock_angle_; - - if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { - *lifting_left_control_angle = lifting_down_angle_left_; - *lifting_right_control_angle = lifting_down_angle_right_; - } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { - *lifting_left_control_angle = lifting_up_angle_left_; - *lifting_right_control_angle = lifting_up_angle_right_; - } - - // if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { - // *trigger_control_angle = trigger_lock_angle_; - // } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { - // *trigger_control_angle = trigger_free_angle_; - // } - } - -private: - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - - uint16_t trigger_lock_angle_; - uint16_t trigger_free_angle_; - OutputInterface trigger_control_angle; - OutputInterface lifting_left_control_angle; - OutputInterface lifting_right_control_angle; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_middle_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_middle_angle_right_; - uint16_t lifting_down_angle_right_; - - -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ServoTest, rmcs_executor::Component) - -// FF FF 01 03 03 16 E2 -// FF FF 01 04 02 16 02 E0 - -// FF FF 01 03 03 18 E0 -// FF FF 01 04 02 18 02 DE - -// FF FF 01 05 03 1C 00 00 DB -// FF FF 01 05 03 1C 00 01 DA - -// FF FF 01 07 03 41 00 14 00 00 9F -// FF FF 01 07 03 41 00 32 00 00 81 - -// FF FF 01 05 03 41 00 14 A1 -// FF FF 01 04 02 1D 01 DA \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp deleted file mode 100644 index 0a4913a3..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include -#include -#include -#include -#include -#include - -/* -angle controls & launch parameter settings -Version: 1.0.0 (manual control) -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -class DartSettings - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartSettings() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - log_enable_ = get_parameter("log_enable").as_bool(); - force_screw_max_velocity_ = get_parameter("screw_max_velocity").as_double(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/force_sensor/weight/channel1", force_sensor_ch1_weight_); - register_input("/force_sensor/weight/channel2", force_sensor_ch2_weight_); - - register_output("/dart/force_screw_motor/control_velocity", force_screw_control_velocity_); - register_output("/dart/yaw_motor/control_velocity", yaw_control_velocity_); - register_output("/dart/pitch_motor/control_velocity", pitch_control_velocity_); - } - - void update() override { - if (*switch_left_ == rmcs_msgs::Switch::UP) { - if (*switch_right_ == rmcs_msgs::Switch::DOWN) { - *force_screw_control_velocity_ = joystick_right_->x() * force_screw_max_velocity_; - } - if (*switch_right_ == rmcs_msgs::Switch::MIDDLE) { - *yaw_control_velocity_ = joystick_right_->y() * angle_control_max_velocity_; - *pitch_control_velocity_ = joystick_left_->x() * angle_control_max_velocity_; - } - } else { - reset(); - } - - if (log_enable_) { - force_sensor_measurement_log(); - } - } - -private: - void reset() { - *force_screw_control_velocity_ = 0; - *yaw_control_velocity_ = nan_; - *pitch_control_velocity_ = nan_; - } - - void force_sensor_measurement_log() { - if (log_count_++ > 100) { - log_count_ = 0; - RCLCPP_INFO( - logger_, "ch1: %d | ch2: %d", *force_sensor_ch1_weight_, *force_sensor_ch2_weight_); - } - } - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - - OutputInterface yaw_control_velocity_; - OutputInterface pitch_control_velocity_; - - InputInterface force_sensor_ch1_weight_, force_sensor_ch2_weight_; - OutputInterface force_screw_control_velocity_; - double force_screw_max_velocity_; - double angle_control_max_velocity_; - - bool log_enable_ = false; - int log_count_ = 0; -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp deleted file mode 100644 index 3a068201..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "controller/pid/matrix_pid_calculator.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -class DartLaunchController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchController() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) - , drive_belt_pid_calculator_( - get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()) { - - dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - - launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); - launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - - register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); - register_output( - "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); - - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - } - - void update() override { - using namespace rmcs_msgs; - - if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) - && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { - *launch_stage_ = DartLaunchStages::DISABLE; - reset_all_controls(); - - } else if (*switch_left_ == Switch::MIDDLE) { - - if (last_launch_stage_ == DartLaunchStages::DISABLE) { - *launch_stage_ = DartLaunchStages::RESETTING; - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - if (last_launch_stage_ == DartLaunchStages::INIT) { - *launch_stage_ = DartLaunchStages::LOADING; - } else if (last_launch_stage_ == DartLaunchStages::READY) { - *launch_stage_ = DartLaunchStages::CANCEL; - } - } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - if (last_launch_stage_ == DartLaunchStages::READY) { - *launch_stage_ = DartLaunchStages::INIT; - trigger_lock_flag_ = false; - } else { - RCLCPP_INFO(logger_, "Dart has't been loaded !"); - } - } - - if (blocking_detection()) { - if (last_launch_stage_ == DartLaunchStages::LOADING) { - *launch_stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = true; - dirve_belt_block_count_ = 0; - - } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { - *launch_stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = false; - dirve_belt_block_count_ = 0; - - } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { - *launch_stage_ = - trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; - dirve_belt_block_count_ = 0; - } - } - double control_velocity = 0; - double control_torque_limit = max_control_torque_; - - if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { - control_velocity = -dirve_belt_working_velocity_; - control_torque_limit = max_control_torque_ * 0.1; - } else if ( - *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING - || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { - control_velocity = dirve_belt_working_velocity_; - control_torque_limit = max_control_torque_; - } else { - control_velocity = 0; - } - drive_belt_sync_control(control_velocity, control_torque_limit); - // RCLCPP_INFO(logger_, "%lf", control_velocity); - } - - *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; - - last_switch_left_ = *switch_left_; - last_switch_right_ = *switch_right_; - last_launch_stage_ = *launch_stage_; - } - -private: - void reset_all_controls() { - *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - } - - void drive_belt_sync_control(double set_velocity, double control_torque_limit) { - if (set_velocity == 0) { - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; - - Eigen::Vector2d relative_velocity{ - *left_drive_belt_velocity_ - *right_drive_belt_velocity_, - *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - - Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - - *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -control_torque_limit, control_torque_limit); - *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -control_torque_limit, control_torque_limit); - } - - bool blocking_detection() { - if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) - || (abs(*right_drive_belt_velocity_) < 0.5 - && abs(*right_drive_belt_control_torque_) > 0.5)) { - dirve_belt_block_count_++; - } - - return dirve_belt_block_count_ > 1000 ? true : false; - } - - int dirve_belt_block_count_ = 0; - double dirve_belt_working_velocity_; - - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_; - rmcs_msgs::Switch last_switch_left_; - - OutputInterface left_drive_belt_control_torque_; - OutputInterface right_drive_belt_control_torque_; - InputInterface left_drive_belt_velocity_; - InputInterface right_drive_belt_velocity_; - - double max_control_torque_; - - OutputInterface launch_stage_; - rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - - uint16_t launch_lock_angle_; - uint16_t launch_trigger_angle_; - bool trigger_lock_flag_ = false; - OutputInterface trigger_control_angle; - - pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; - double sync_coefficient_; -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index d8c2d5e8..b0ba0d2d 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -1,567 +1,449 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" -// #include "hardware/device/trigger_servo.hpp" - -// #include "filter/low_pass_filter.hpp" -// #include "hardware/device/bmi088.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/force_sensor_runtime.hpp" -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include - -// #include - -// // #include -// // #include -// // #include -// // #include - -// namespace rmcs_core::hardware { - -// class CatapultDart -// : public rmcs_executor::Component -// , public rclcpp::Node -// , public librmcs::agent::CBoard { -// public: -// CatapultDart() -// : Node{get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , -// librmcs::agent::CBoard{static_cast(get_parameter("usb_pid").as_int())} , -// logger_(get_logger()) , pitch_angle_filter_(20.0, 1000.0) , pitch_velocity_filter_(20.0, -// 1000.0) , yaw_velocity_filter_(20.0, 1000.0) , yaw_filter_(5.0, 1000.0) , -// pitch_filter_(10.0, 1000.0) , roll_filter_(10.0, 1000.0) , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , dr16_(*this) -// , imu_(1000, 0.2, 0.0) -// , pitch_motor_( -// *this, *dart_command_, "/dart/pitch_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , yaw_motor_( -// *this, *dart_command_, "/dart/yaw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , force_control_motor_( -// *this, *dart_command_, "/dart/force_control_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , drive_belt_motor_( -// {*this, *dart_command_, "/dart/drive_belt/left", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, -// {*this, *dart_command_, "/dart/drive_belt/right", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508} -// .set_reduction_ratio(19.) -// .set_reversed()}) -// , force_sensor_(*this) -// , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) -// , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) -// , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) -// , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { -// register_output("/tf", tf_); -// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); -// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); -// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); -// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); -// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - -// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); -// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - -// setup_imu_coordinate_mapping(); -// imu_sampler_initialize(); -// tf_broadcaster_ = std::make_unique(*this); -// start_time_ = std::chrono::steady_clock::now(); - -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); -// }); -// limiting_calibrate_subscription_ = create_subscription( -// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) -// { -// trigger_servo_calibrate_subscription_callback( -// limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); -// }); -// lifting_left_calibrate_subscription_ = create_subscription( -// "/lifting_left/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// lifting_left_, std::move(msg), lifting_left_uart_data_ptr); -// }); -// lifting_right_calibrate_subscription_ = create_subscription( -// "/lifting_right/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// lifting_right_, std::move(msg), lifting_right_uart_data_ptr); -// }); - -// 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; -// }; - -// last_read_left_time_ = this->now(); -// last_read_right_time_ = this->now(); -// } - -// ~CatapultDart() override { -// stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_[0].update_status(); -// drive_belt_motor_[1].update_status(); -// force_control_motor_.update_status(); -// force_sensor_.update_status(); - -// imu_.update_status(); -// processImuData(); -// } - -// void command_update() { -// uint16_t can_commands[4]; - -// if (pub_time_count_++ > 100) { -// transmit_buffer_.add_can1_transmission( -// 0x301, std::bit_cast(force_sensor_.generate_command())); -// pub_time_count_ = 0; -// } - -// can_commands[0] = pitch_motor_.generate_command(); -// can_commands[1] = yaw_motor_.generate_command(); -// can_commands[2] = force_control_motor_.generate_command(); -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - -// can_commands[0] = drive_belt_motor_[0].generate_command(); -// can_commands[1] = drive_belt_motor_[1].generate_command(); -// can_commands[2] = 0; -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - -// if (!trigger_servo_.calibrate_mode()) { -// uint16_t current_target = trigger_servo_.get_target_angle(); -// if (current_target != last_trigger_angle_) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission( -// trigger_servo_uart_data_ptr, uart_data_length); -// last_trigger_angle_ = current_target; -// } -// } - -// if (!limiting_servo_.calibrate_mode()) { -// uint16_t current_target = limiting_servo_.get_target_angle(); -// if (current_target != last_limiting_angle_) { -// size_t uart_data_length; -// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_limiting_angle_ = current_target; -// } -// } - -// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { -// uint16_t current_target_left = lifting_left_.get_target_angle(); -// uint16_t current_target_right = lifting_right_.get_target_angle(); -// if (current_target_left != last_lifting_left_angle_) { -// size_t uart_data_length; -// uint16_t runtime_left = 0; -// uint16_t runtime_right = 0; -// std::unique_ptr command_buffer = -// device::TriggerServo::generate_sync_run_command( -// uart_data_length, LIFTING_LEFT_ID, LIFTING_RIGHT_ID, current_target_left, -// current_target_right, runtime_left, runtime_right); -// const auto lifting_table_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission( -// lifting_table_uart_data_ptr, uart_data_length); -// last_lifting_left_angle_ = current_target_left; -// } -// } - -// auto now = this->now(); - -// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { -// size_t uart_data_length; -// auto command_buffer = lifting_left_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_left_time_ = now; -// } -// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) -// { -// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { -// size_t uart_data_length; -// auto command_buffer = lifting_right_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, -// uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_right_time_ = now; -// } -// } - -// transmit_buffer_.trigger_transmission(); -// } - -// int pub_time_count_ = 0; - -// 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 == 0x302) { -// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] -// return; - -// if (can_id == 0x201) { -// pitch_motor_.store_status(can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(can_data); -// } else if (can_id == 0x203) { -// force_control_motor_.store_status(can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_[0].store_status(can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_[1].store_status(can_data); -// } -// } - -// void uart1_receive_callback(const std::byte* data, uint8_t length) override { -// referee_ring_buffer_receive_.emplace_back_multi( -// [&data](std::byte* storage) { *storage = *data++; }, length); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// std::string hex_string = bytes_to_hex_string(data, length); -// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, -// hex_string.c_str()); - -// if (length < 3) { -// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); -// return; -// } - -// uint8_t servo_id = static_cast(data[2]); -// std::pair result{false, 0}; - -// switch (servo_id) { -// case TRIGGER_SERVO_ID: -// result = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIMITING_SERVO_ID: -// result = limiting_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_LEFT_ID: -// result = lifting_left_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_left_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_RIGHT_ID: -// result = lifting_right_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_right_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); break; -// } -// } - -// 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 { -// imu_.store_accelerometer_status(x, y, z); -// } - -// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_gyroscope_status(x, y, z); -// } - -// private: -// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; -// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; -// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; -// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; -// std::byte* trigger_servo_uart_data_ptr; -// std::byte* limiting_servo_uart_data_ptr; -// std::byte* lifting_left_uart_data_ptr; -// std::byte* lifting_right_uart_data_ptr; - -// void trigger_servo_calibrate_subscription_callback( -// device::TriggerServo& servo_, std_msgs::msg::Int32::UniquePtr msg, -// std::byte* servo_uart_data_ptr) { -// /* -// 标定命令格式： -// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" -// 替换data值就行 -// */ -// servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - -// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); -// RCLCPP_INFO( -// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, -// hex_string.c_str()); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } // DEBUG TOOL - -// void setup_imu_coordinate_mapping() { -// imu_.set_coordinate_mapping( -// [](double x, double y, double z) -> std::tuple { -// return {x, -y, -z}; -// }); -// } - -// void imu_sampler_initialize() { -// start_time_ = std::chrono::steady_clock::now(); -// yaw_drift_coefficient_ = 0.0; -// } - -// void processImuData() { -// auto current_time = std::chrono::steady_clock::now(); -// double elapsed_seconds = std::chrono::duration(current_time - -// start_time_).count(); - -// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); -// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - -// double roll = std::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); -// double pitch = std::asin(-rotation_matrix(2, 0)); -// double yaw = std::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)); - -// double transformed_roll = -roll_filter_.update(roll); -// double transformed_pitch = pitch_filter_.update(pitch); -// double transformed_yaw = -yaw_filter_.update(yaw); - -// if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { -// yaw_samples_.push_back(transformed_yaw); -// time_samples_.push_back(elapsed_seconds); -// double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; -// double n = static_cast(yaw_samples_.size()); - -// for (int i = 0; i < n; ++i) { -// sum_x += time_samples_[i]; -// sum_y += yaw_samples_[i]; -// sum_xy += time_samples_[i] * yaw_samples_[i]; -// sum_xx += time_samples_[i] * time_samples_[i]; -// } - -// double denominator = n * sum_xx - sum_x * sum_x; -// if (std::abs(denominator) > 1e-10) { -// yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; -// } -// } - -// transformed_yaw -= ((yaw_drift_coefficient_ + 0.000512) * elapsed_seconds); -// publishTfTransforms(transformed_roll, transformed_pitch, transformed_yaw); - -// *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; -// *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; -// *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; -// } - -// void publishTfTransforms(double roll, double pitch, double yaw) { -// auto now = this->get_clock()->now(); - -// auto create_transform = [&](const std::string& parent, const std::string& child, -// const geometry_msgs::msg::Vector3& trans, -// const geometry_msgs::msg::Quaternion& rot) { -// geometry_msgs::msg::TransformStamped tf; -// tf.header.stamp = now; -// tf.header.frame_id = parent; -// tf.child_frame_id = child; -// tf.transform.translation = trans; -// tf.transform.rotation = rot; -// return tf; -// }; - -// auto create_translation = [](double x, double y, double z) { -// geometry_msgs::msg::Vector3 t; -// t.x = x; -// t.y = y; -// t.z = z; -// return t; -// }; - -// auto create_rotation = [](double x, double y, double z, double w) { -// geometry_msgs::msg::Quaternion r; -// r.x = x; -// r.y = y; -// r.z = z; -// r.w = w; -// return r; -// }; - -// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); -// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - -// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); -// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); -// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); -// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - -// tf_broadcaster_->sendTransform(create_transform( -// "base_link", "gimbal_center_link", zero_trans, create_rotation(0, 0, 0, 1))); -// tf_broadcaster_->sendTransform(create_transform( -// "gimbal_center_link", "yaw_link", zero_trans, -// create_rotation(yaw_quat.x(), yaw_quat.y(), yaw_quat.z(), yaw_quat.w()))); -// tf_broadcaster_->sendTransform(create_transform( -// "yaw_link", "pitch_link", pitch_trans, -// create_rotation(pitch_quat.x(), pitch_quat.y(), pitch_quat.z(), pitch_quat.w()))); -// tf_broadcaster_->sendTransform(create_transform( -// "pitch_link", "odom_imu", zero_trans, -// create_rotation( -// combined_quaternion.x(), combined_quaternion.y(), combined_quaternion.z(), -// combined_quaternion.w()))); -// tf_broadcaster_->sendTransform( -// create_transform("world", "base_link", zero_trans, create_rotation(0, 0, 0, 1))); -// } - -// rclcpp::Logger logger_; -// filter::LowPassFilter<1> pitch_angle_filter_; -// filter::LowPassFilter<1> pitch_velocity_filter_; -// filter::LowPassFilter<1> yaw_velocity_filter_; -// filter::LowPassFilter<1> yaw_filter_; -// filter::LowPassFilter<1> pitch_filter_; -// filter::LowPassFilter<1> roll_filter_; -// std::chrono::steady_clock::time_point start_time_; -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDart& robot) -// : dart_(robot) {} -// void update() override { dart_.command_update(); } -// CatapultDart& dart_; -// }; - -// std::shared_ptr dart_command_; -// double first_sample_spot_; -// double final_sample_spot_; -// OutputInterface tf_; -// std::unique_ptr tf_broadcaster_; -// device::Dr16 dr16_; -// device::Bmi088 imu_; -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_control_motor_; -// device::DjiMotor drive_belt_motor_[2]; -// device::ForceSensorRuntime force_sensor_; -// device::TriggerServo trigger_servo_; -// device::TriggerServo limiting_servo_; -// device::TriggerServo lifting_left_; -// device::TriggerServo lifting_right_; -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; -// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; -// OutputInterface referee_serial_; -// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; -// std::thread event_thread_; -// OutputInterface catapult_pitch_angle_; -// OutputInterface catapult_roll_angle_; -// OutputInterface catapult_yaw_angle_; -// OutputInterface lifting_current_angle_left_; -// OutputInterface lifting_current_angle_right_; -// double yaw_drift_coefficient_ = 0.0; -// std::vector yaw_samples_, time_samples_; -// uint16_t last_trigger_angle_ = 0xFFFF; -// uint16_t last_limiting_angle_ = 0xFFFF; -// uint16_t last_lifting_left_angle_ = 0xFFFF; -// rclcpp::Time last_read_left_time_; -// rclcpp::Time last_read_right_time_; -// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -// }; -// } // namespace rmcs_core::hardware - -// #include -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "filter/low_pass_filter.hpp" +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/force_sensor.hpp" +#include "hardware/device/pwm_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +namespace rmcs_core::hardware { + +class CatapultDartV4 + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartV4() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , pitch_angle_filter_(20.0, 1000.0) + , pitch_velocity_filter_(20.0, 1000.0) + , yaw_velocity_filter_(20.0, 1000.0) + , yaw_filter_(5.0, 1000.0) + , pitch_filter_(10.0, 1000.0) + , roll_filter_(10.0, 1000.0) + , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} + , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} + , force_screw_motor_{*this, *dart_command_, "/dart/force_screw"} + , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} + , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} + , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} + , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} + , force_sensor_{*this} + , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} + + , dr16_{*this} + , imu_{1000, 0.2, 0.0} { + + pitch_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + yaw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + force_screw_motor_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_left_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); + drive_belt_motor_right_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(19.)); + + first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); + final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); + + setup_imu_coordinate_mapping(); + start_time_ = std::chrono::steady_clock::now(); + yaw_drift_coefficient_ = 0.0; + tf_broadcaster_ = std::make_unique(*this); + + register_output("/tf", tf_); + register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + + lifting_left_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + lifting_right_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + + force_sensor_calibrate_subscription_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + if (msg->data == 0) { + RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); + auto board = start_transmit(); + board.can1_transmit({ + .can_id = 0x201, + .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), + }); + } else { + RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); + } + } + ); + + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { + std::lock_guard lock(referee_mutex_); + size_t count = 0; + while (count < size && !referee_ring_buffer_receive_.empty()) { + buffer[count++] = referee_ring_buffer_receive_.front(); + referee_ring_buffer_receive_.pop_front(); + } + return count; + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { + auto board = start_transmit(); + board.uart1_transmit({.uart_data = std::span{buffer, size}}); + return size; + }; + } + + CatapultDartV4(const CatapultDartV4&) = delete; + CatapultDartV4& operator=(const CatapultDartV4&) = delete; + CatapultDartV4(CatapultDartV4&&) = delete; + CatapultDartV4& operator=(CatapultDartV4&&) = delete; + + ~CatapultDartV4() override = default; + + void update() override { + dr16_.update_status(); + pitch_motor_.update_status(); + yaw_motor_.update_status(); + drive_belt_motor_left_.update_status(); + drive_belt_motor_right_.update_status(); + force_screw_motor_.update_status(); + force_sensor_.update_status(); + imu_.update_status(); + processImuData(); + } + + void command_update() { + auto board = start_transmit(); + + board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); + + if (pub_time_count_++ > 100) { + board.can1_transmit({ + .can_id = 0x301, + .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), + }); + pub_time_count_ = 0; + } + + board.can1_transmit({ + .can_id = 0x141, + .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), + }); + + board.can1_transmit({ + .can_id = 0x145, + .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), + }); + + board.can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + pitch_motor_.generate_command(), + yaw_motor_.generate_command(), + force_screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + + }); + + board.can2_transmit({ + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + drive_belt_motor_left_.generate_command(), + drive_belt_motor_right_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + }.as_bytes(), + }); + } + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + const auto can_id = data.can_id; + + if (can_id == 0x302) { + force_sensor_frame_count_++; + if (force_sensor_frame_count_ % 200 == 1) { + RCLCPP_INFO( + logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", + data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + force_sensor_.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) [[unlikely]] + return; + const auto can_id = data.can_id; + + // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID + if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 + && can_id != 0x205 && can_id != 0x206) { + can2_unknown_count_++; + if (can2_unknown_count_ % 50 == 1) { + RCLCPP_INFO( + logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", + can_id, data.can_data.size(), + bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); + } + } + + if (can_id == 0x201) { + pitch_motor_.store_status(data.can_data); + } else if (can_id == 0x202) { + yaw_motor_.store_status(data.can_data); + } else if (can_id == 0x203) { + force_screw_motor_.store_status(data.can_data); + } else if (can_id == 0x205) { + drive_belt_motor_left_.store_status(data.can_data); + } else if (can_id == 0x206) { + drive_belt_motor_right_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + std::lock_guard lock(referee_mutex_); + for (auto byte : data.uart_data) { + referee_ring_buffer_receive_.push_back(byte); + } + } + + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + } + + 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 { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + +private: + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) + return "[]"; + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + for (size_t i = 0; i < size; ++i) + ss << std::setw(2) << static_cast(data[i]) << " "; + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') + result.pop_back(); + return result; + } + + void setup_imu_coordinate_mapping() { + imu_.set_coordinate_mapping( + [](double x, double y, double z) -> std::tuple { + return {x, -y, -z}; + }); + } + + void processImuData() { + auto current_time = std::chrono::steady_clock::now(); + double elapsed_seconds = + std::chrono::duration(current_time - start_time_).count(); + + Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); + Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); + + double roll = std::atan2(rot(2, 1), rot(2, 2)); + double pitch = std::asin(-rot(2, 0)); + double yaw = std::atan2(rot(1, 0), rot(0, 0)); + + double t_roll = -roll_filter_.update(roll); + double t_pitch = pitch_filter_.update(pitch); + double t_yaw = -yaw_filter_.update(yaw); + + if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { + yaw_samples_.push_back(t_yaw); + time_samples_.push_back(elapsed_seconds); + + double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; + double n = static_cast(yaw_samples_.size()); + for (size_t i = 0; i < yaw_samples_.size(); ++i) { + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; + sum_xy += time_samples_[i] * yaw_samples_[i]; + sum_xx += time_samples_[i] * time_samples_[i]; + } + double denominator = n * sum_xx - sum_x * sum_x; + if (std::abs(denominator) > 1e-10) + yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; + } + + t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; + publishTfTransforms(t_roll, t_pitch, t_yaw); + + *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; + *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; + *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; + } + + void publishTfTransforms(double roll, double pitch, double yaw) { + auto now = this->get_clock()->now(); + + auto make_tf = + [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; + + auto make_vec = [](double x, double y, double z) { + geometry_msgs::msg::Vector3 v; + v.x = x; v.y = y; v.z = z; + return v; + }; + + auto make_quat = [](double x, double y, double z, double w) { + geometry_msgs::msg::Quaternion q; + q.x = x; q.y = y; q.z = z; q.w = w; + return q; + }; + + Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); + Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; + + auto zero = make_vec(0, 0, 0); + auto p_trans = make_vec(0, 0, 0.05); + auto id_rot = make_quat(0, 0, 0, 1); + + tf_broadcaster_->sendTransform( + make_tf("base_link", "gimbal_center_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf( + "gimbal_center_link", "yaw_link", zero, + make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "yaw_link", "pitch_link", p_trans, + make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); + tf_broadcaster_->sendTransform(make_tf( + "pitch_link", "odom_imu", zero, + make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); + tf_broadcaster_->sendTransform( + make_tf("world", "base_link", zero, id_rot)); + } + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartV4& dart) + : dart_(dart) {} + void update() override { dart_.command_update();} + + private: + CatapultDartV4& dart_; + }; + + std::shared_ptr dart_command_; + rclcpp::Logger logger_; + + filter::LowPassFilter<1> pitch_angle_filter_; + filter::LowPassFilter<1> pitch_velocity_filter_; + filter::LowPassFilter<1> yaw_velocity_filter_; + filter::LowPassFilter<1> yaw_filter_; + filter::LowPassFilter<1> pitch_filter_; + filter::LowPassFilter<1> roll_filter_; + + std::chrono::steady_clock::time_point start_time_; + + device::DjiMotor pitch_motor_; + device::DjiMotor yaw_motor_; + device::DjiMotor force_screw_motor_; + device::DjiMotor drive_belt_motor_left_; + device::DjiMotor drive_belt_motor_right_; + + device::LkMotor lifting_left_motor_; + device::LkMotor lifting_right_motor_; + + device::ForceSensor force_sensor_; + + device::PWMServo trigger_servo_; + + device::Dr16 dr16_; + device::Bmi088 imu_; + + std::unique_ptr tf_broadcaster_; + + OutputInterface tf_; + OutputInterface catapult_pitch_angle_; + OutputInterface catapult_roll_angle_; + OutputInterface catapult_yaw_angle_; + rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; + + std::mutex referee_mutex_; + std::deque referee_ring_buffer_receive_; + OutputInterface referee_serial_; + + int pub_time_count_ = 0; + int force_sensor_frame_count_ = 0; + int can2_unknown_count_ = 0; + + double first_sample_spot_ = 1.0; + double final_sample_spot_ = 4.0; + double yaw_drift_coefficient_ = 0.0; + std::vector yaw_samples_; + std::vector time_samples_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV4, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp deleted file mode 100644 index 9c6eb2ae..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" - -// namespace rmcs_core::hardware { - -// class CatapultDartforlibrmcsv3 -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::agent::CBoard { -// public: -// CatapultDartforlibrmcsv3() -// : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} -// , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , logger_{get_logger()} -// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} -// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} -// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} -// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} -// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} -// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} -// , force_sensor_{*this} { - -// left_belt_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// right_belt_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(19.)); - -// yaw_control_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// pitch_control_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// screw_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// } - -// CatapultDartforlibrmcsv3(const CatapultDartforlibrmcsv3&) = delete; -// CatapultDartforlibrmcsv3& operator=(const CatapultDartforlibrmcsv3&) = delete; -// CatapultDartforlibrmcsv3(CatapultDartforlibrmcsv3&&) = delete; -// CatapultDartforlibrmcsv3& operator=(CatapultDartforlibrmcsv3&&) = delete; - -// ~CatapultDartforlibrmcsv3() override = default; - -// void update() override { force_sensor_.update_status(); } - -// void command_update() { -// auto board = start_transmit(); -// board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - -// board.can2_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// pitch_control_motor_.generate_command(), -// yaw_control_motor_.generate_command(), -// screw_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// board.can2_transmit({ -// .can_id = 0x1FF, -// .can_data = -// device::CanPacket8{ -// left_belt_motor_.generate_command(), -// right_belt_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// if (pub_time_count_++ > 100) { -// board.can2_transmit({ -// .can_id = 0x301, -// .can_data = device::CanPacket8{device::ForceSensor::generate_command()}.as_bytes(), -// }); -// pub_time_count_ = 0; -// } -// } - -// protected: -// void can2_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; - -// auto can_id = data.can_id; -// if (can_id == 0x201) { -// pitch_control_motor_.store_status(data.can_data); -// } else if (can_id == 0x202) { -// yaw_control_motor_.store_status(data.can_data); -// } else if (can_id == 0x203) { -// screw_motor_.store_status(data.can_data); -// } else if (can_id == 0x205) { -// left_belt_motor_.store_status(data.can_data); -// } else if (can_id == 0x206) { -// right_belt_motor_.store_status(data.can_data); -// } else if (can_id == 0x302) { -// force_sensor_.store_status(data.can_data); -// } -// } - -// private: -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDartforlibrmcsv3& dart) -// : dart_(dart) {} - -// void update() override { dart_.command_update(); } - -// private: -// CatapultDartforlibrmcsv3& dart_; -// }; -// std::shared_ptr dart_command_; - -// rclcpp::Logger logger_; - -// device::DjiMotor left_belt_motor_, right_belt_motor_; -// device::DjiMotor yaw_control_motor_, pitch_control_motor_; -// device::DjiMotor screw_motor_; -// device::PWMServo trigger_servo_; -// device::ForceSensor force_sensor_; -// int pub_time_count_ = 0; -// }; -// } // namespace rmcs_core::hardware - -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartforlibrmcsv3, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v2.cpp similarity index 100% rename from rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_lk.cpp rename to rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v2.cpp diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp deleted file mode 100644 index 1419d0a8..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp +++ /dev/null @@ -1,426 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "filter/low_pass_filter.hpp" -#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/force_sensor.hpp" -#include "hardware/device/pwm_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartV3Full - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartV3Full() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , pitch_angle_filter_(20.0, 1000.0) - , pitch_velocity_filter_(20.0, 1000.0) - , yaw_velocity_filter_(20.0, 1000.0) - , yaw_filter_(5.0, 1000.0) - , pitch_filter_(10.0, 1000.0) - , roll_filter_(10.0, 1000.0) - , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} - , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} - , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} - , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} - , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} - , force_sensor_{*this} - , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} - - , dr16_{*this} - , imu_{1000, 0.2, 0.0} { - - pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - force_screw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_left_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_right_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - - first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); - final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - - setup_imu_coordinate_mapping(); - start_time_ = std::chrono::steady_clock::now(); - yaw_drift_coefficient_ = 0.0; - tf_broadcaster_ = std::make_unique(*this); - - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - - force_sensor_calibrate_subscription_ = create_subscription( - "/force_sensor/calibrate", rclcpp::QoS{0}, - [this](std_msgs::msg::Int32::UniquePtr&& msg) { - if (msg->data == 0) { - RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); - auto board = start_transmit(); - board.can1_transmit({ - .can_id = 0x201, - .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), - }); - } else { - RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); - } - } - ); - - register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { - std::lock_guard lock(referee_mutex_); - size_t count = 0; - while (count < size && !referee_ring_buffer_receive_.empty()) { - buffer[count++] = referee_ring_buffer_receive_.front(); - referee_ring_buffer_receive_.pop_front(); - } - return count; - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { - auto board = start_transmit(); - board.uart1_transmit({.uart_data = std::span{buffer, size}}); - return size; - }; - } - - CatapultDartV3Full(const CatapultDartV3Full&) = delete; - CatapultDartV3Full& operator=(const CatapultDartV3Full&) = delete; - CatapultDartV3Full(CatapultDartV3Full&&) = delete; - CatapultDartV3Full& operator=(CatapultDartV3Full&&) = delete; - - ~CatapultDartV3Full() override = default; - - void update() override { - dr16_.update_status(); - pitch_motor_.update_status(); - yaw_motor_.update_status(); - drive_belt_motor_left_.update_status(); - drive_belt_motor_right_.update_status(); - force_screw_motor_.update_status(); - force_sensor_.update_status(); - imu_.update_status(); - processImuData(); - } - - void command_update() { - auto board = start_transmit(); - - board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - - if (pub_time_count_++ > 100) { - board.can1_transmit({ - .can_id = 0x301, - .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), - }); - pub_time_count_ = 0; - } - - board.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - pitch_motor_.generate_command(), - yaw_motor_.generate_command(), - force_screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - - }); - - board.can2_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - drive_belt_motor_left_.generate_command(), - drive_belt_motor_right_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - }); - } - -protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - const auto can_id = data.can_id; - - if (can_id == 0x302) { - force_sensor_frame_count_++; - if (force_sensor_frame_count_ % 200 == 1) { - RCLCPP_INFO( - logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", - data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - force_sensor_.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) [[unlikely]] - return; - const auto can_id = data.can_id; - - // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID - if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 - && can_id != 0x205 && can_id != 0x206) { - can2_unknown_count_++; - if (can2_unknown_count_ % 50 == 1) { - RCLCPP_INFO( - logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", - can_id, data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - } - - if (can_id == 0x201) { - pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x203) { - force_screw_motor_.store_status(data.can_data); - } else if (can_id == 0x205) { - drive_belt_motor_left_.store_status(data.can_data); - } else if (can_id == 0x206) { - drive_belt_motor_right_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - std::lock_guard lock(referee_mutex_); - for (auto byte : data.uart_data) { - referee_ring_buffer_receive_.push_back(byte); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - } - - 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 { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - -private: - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) - return "[]"; - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - for (size_t i = 0; i < size; ++i) - ss << std::setw(2) << static_cast(data[i]) << " "; - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') - result.pop_back(); - return result; - } - - void setup_imu_coordinate_mapping() { - imu_.set_coordinate_mapping( - [](double x, double y, double z) -> std::tuple { - return {x, -y, -z}; - }); - } - - void processImuData() { - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = - std::chrono::duration(current_time - start_time_).count(); - - Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - - double roll = std::atan2(rot(2, 1), rot(2, 2)); - double pitch = std::asin(-rot(2, 0)); - double yaw = std::atan2(rot(1, 0), rot(0, 0)); - - double t_roll = -roll_filter_.update(roll); - double t_pitch = pitch_filter_.update(pitch); - double t_yaw = -yaw_filter_.update(yaw); - - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - yaw_samples_.push_back(t_yaw); - time_samples_.push_back(elapsed_seconds); - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = static_cast(yaw_samples_.size()); - for (size_t i = 0; i < yaw_samples_.size(); ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) - yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; - } - - t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; - publishTfTransforms(t_roll, t_pitch, t_yaw); - - *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; - *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; - } - - void publishTfTransforms(double roll, double pitch, double yaw) { - auto now = this->get_clock()->now(); - - auto make_tf = - [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; - - auto make_vec = [](double x, double y, double z) { - geometry_msgs::msg::Vector3 v; - v.x = x; v.y = y; v.z = z; - return v; - }; - - auto make_quat = [](double x, double y, double z, double w) { - geometry_msgs::msg::Quaternion q; - q.x = x; q.y = y; q.z = z; q.w = w; - return q; - }; - - Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); - Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - - auto zero = make_vec(0, 0, 0); - auto p_trans = make_vec(0, 0, 0.05); - auto id_rot = make_quat(0, 0, 0, 1); - - tf_broadcaster_->sendTransform( - make_tf("base_link", "gimbal_center_link", zero, id_rot)); - tf_broadcaster_->sendTransform(make_tf( - "gimbal_center_link", "yaw_link", zero, - make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "yaw_link", "pitch_link", p_trans, - make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "pitch_link", "odom_imu", zero, - make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); - tf_broadcaster_->sendTransform( - make_tf("world", "base_link", zero, id_rot)); - } - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartV3Full& dart) - : dart_(dart) {} - void update() override { dart_.command_update();} - - private: - CatapultDartV3Full& dart_; - }; - - std::shared_ptr dart_command_; - rclcpp::Logger logger_; - - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - filter::LowPassFilter<1> yaw_filter_; - filter::LowPassFilter<1> pitch_filter_; - filter::LowPassFilter<1> roll_filter_; - - std::chrono::steady_clock::time_point start_time_; - - device::DjiMotor pitch_motor_; - device::DjiMotor yaw_motor_; - device::DjiMotor force_screw_motor_; - device::DjiMotor drive_belt_motor_left_; - device::DjiMotor drive_belt_motor_right_; - - device::ForceSensor force_sensor_; - - device::PWMServo trigger_servo_; - - device::Dr16 dr16_; - device::Bmi088 imu_; - - std::unique_ptr tf_broadcaster_; - - OutputInterface tf_; - OutputInterface catapult_pitch_angle_; - OutputInterface catapult_roll_angle_; - OutputInterface catapult_yaw_angle_; - rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; - - std::mutex referee_mutex_; - std::deque referee_ring_buffer_receive_; - OutputInterface referee_serial_; - - int pub_time_count_ = 0; - int force_sensor_frame_count_ = 0; - int can2_unknown_count_ = 0; - - double first_sample_spot_ = 1.0; - double final_sample_spot_ = 4.0; - double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_; - std::vector time_samples_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp deleted file mode 100644 index b0ba0d2d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp +++ /dev/null @@ -1,449 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "filter/low_pass_filter.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/force_sensor.hpp" -#include "hardware/device/pwm_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartV4 - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartV4() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , pitch_angle_filter_(20.0, 1000.0) - , pitch_velocity_filter_(20.0, 1000.0) - , yaw_velocity_filter_(20.0, 1000.0) - , yaw_filter_(5.0, 1000.0) - , pitch_filter_(10.0, 1000.0) - , roll_filter_(10.0, 1000.0) - , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} - , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} - , force_screw_motor_{*this, *dart_command_, "/dart/force_screw"} - , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} - , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} - , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} - , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} - , force_sensor_{*this} - , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} - - , dr16_{*this} - , imu_{1000, 0.2, 0.0} { - - pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - force_screw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_left_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_right_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - - first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); - final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - - setup_imu_coordinate_mapping(); - start_time_ = std::chrono::steady_clock::now(); - yaw_drift_coefficient_ = 0.0; - tf_broadcaster_ = std::make_unique(*this); - - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - - lifting_left_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - lifting_right_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - - force_sensor_calibrate_subscription_ = create_subscription( - "/force_sensor/calibrate", rclcpp::QoS{0}, - [this](std_msgs::msg::Int32::UniquePtr&& msg) { - if (msg->data == 0) { - RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); - auto board = start_transmit(); - board.can1_transmit({ - .can_id = 0x201, - .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), - }); - } else { - RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); - } - } - ); - - register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { - std::lock_guard lock(referee_mutex_); - size_t count = 0; - while (count < size && !referee_ring_buffer_receive_.empty()) { - buffer[count++] = referee_ring_buffer_receive_.front(); - referee_ring_buffer_receive_.pop_front(); - } - return count; - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { - auto board = start_transmit(); - board.uart1_transmit({.uart_data = std::span{buffer, size}}); - return size; - }; - } - - CatapultDartV4(const CatapultDartV4&) = delete; - CatapultDartV4& operator=(const CatapultDartV4&) = delete; - CatapultDartV4(CatapultDartV4&&) = delete; - CatapultDartV4& operator=(CatapultDartV4&&) = delete; - - ~CatapultDartV4() override = default; - - void update() override { - dr16_.update_status(); - pitch_motor_.update_status(); - yaw_motor_.update_status(); - drive_belt_motor_left_.update_status(); - drive_belt_motor_right_.update_status(); - force_screw_motor_.update_status(); - force_sensor_.update_status(); - imu_.update_status(); - processImuData(); - } - - void command_update() { - auto board = start_transmit(); - - board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - - if (pub_time_count_++ > 100) { - board.can1_transmit({ - .can_id = 0x301, - .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), - }); - pub_time_count_ = 0; - } - - board.can1_transmit({ - .can_id = 0x141, - .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), - }); - - board.can1_transmit({ - .can_id = 0x145, - .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), - }); - - board.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - pitch_motor_.generate_command(), - yaw_motor_.generate_command(), - force_screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - - }); - - board.can2_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - drive_belt_motor_left_.generate_command(), - drive_belt_motor_right_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - }); - } - -protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - const auto can_id = data.can_id; - - if (can_id == 0x302) { - force_sensor_frame_count_++; - if (force_sensor_frame_count_ % 200 == 1) { - RCLCPP_INFO( - logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", - data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - force_sensor_.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) [[unlikely]] - return; - const auto can_id = data.can_id; - - // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID - if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 - && can_id != 0x205 && can_id != 0x206) { - can2_unknown_count_++; - if (can2_unknown_count_ % 50 == 1) { - RCLCPP_INFO( - logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", - can_id, data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - } - - if (can_id == 0x201) { - pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x203) { - force_screw_motor_.store_status(data.can_data); - } else if (can_id == 0x205) { - drive_belt_motor_left_.store_status(data.can_data); - } else if (can_id == 0x206) { - drive_belt_motor_right_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - std::lock_guard lock(referee_mutex_); - for (auto byte : data.uart_data) { - referee_ring_buffer_receive_.push_back(byte); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - } - - 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 { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - -private: - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) - return "[]"; - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - for (size_t i = 0; i < size; ++i) - ss << std::setw(2) << static_cast(data[i]) << " "; - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') - result.pop_back(); - return result; - } - - void setup_imu_coordinate_mapping() { - imu_.set_coordinate_mapping( - [](double x, double y, double z) -> std::tuple { - return {x, -y, -z}; - }); - } - - void processImuData() { - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = - std::chrono::duration(current_time - start_time_).count(); - - Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - - double roll = std::atan2(rot(2, 1), rot(2, 2)); - double pitch = std::asin(-rot(2, 0)); - double yaw = std::atan2(rot(1, 0), rot(0, 0)); - - double t_roll = -roll_filter_.update(roll); - double t_pitch = pitch_filter_.update(pitch); - double t_yaw = -yaw_filter_.update(yaw); - - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - yaw_samples_.push_back(t_yaw); - time_samples_.push_back(elapsed_seconds); - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = static_cast(yaw_samples_.size()); - for (size_t i = 0; i < yaw_samples_.size(); ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) - yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; - } - - t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; - publishTfTransforms(t_roll, t_pitch, t_yaw); - - *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; - *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; - } - - void publishTfTransforms(double roll, double pitch, double yaw) { - auto now = this->get_clock()->now(); - - auto make_tf = - [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; - - auto make_vec = [](double x, double y, double z) { - geometry_msgs::msg::Vector3 v; - v.x = x; v.y = y; v.z = z; - return v; - }; - - auto make_quat = [](double x, double y, double z, double w) { - geometry_msgs::msg::Quaternion q; - q.x = x; q.y = y; q.z = z; q.w = w; - return q; - }; - - Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); - Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - - auto zero = make_vec(0, 0, 0); - auto p_trans = make_vec(0, 0, 0.05); - auto id_rot = make_quat(0, 0, 0, 1); - - tf_broadcaster_->sendTransform( - make_tf("base_link", "gimbal_center_link", zero, id_rot)); - tf_broadcaster_->sendTransform(make_tf( - "gimbal_center_link", "yaw_link", zero, - make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "yaw_link", "pitch_link", p_trans, - make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "pitch_link", "odom_imu", zero, - make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); - tf_broadcaster_->sendTransform( - make_tf("world", "base_link", zero, id_rot)); - } - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartV4& dart) - : dart_(dart) {} - void update() override { dart_.command_update();} - - private: - CatapultDartV4& dart_; - }; - - std::shared_ptr dart_command_; - rclcpp::Logger logger_; - - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - filter::LowPassFilter<1> yaw_filter_; - filter::LowPassFilter<1> pitch_filter_; - filter::LowPassFilter<1> roll_filter_; - - std::chrono::steady_clock::time_point start_time_; - - device::DjiMotor pitch_motor_; - device::DjiMotor yaw_motor_; - device::DjiMotor force_screw_motor_; - device::DjiMotor drive_belt_motor_left_; - device::DjiMotor drive_belt_motor_right_; - - device::LkMotor lifting_left_motor_; - device::LkMotor lifting_right_motor_; - - device::ForceSensor force_sensor_; - - device::PWMServo trigger_servo_; - - device::Dr16 dr16_; - device::Bmi088 imu_; - - std::unique_ptr tf_broadcaster_; - - OutputInterface tf_; - OutputInterface catapult_pitch_angle_; - OutputInterface catapult_roll_angle_; - OutputInterface catapult_yaw_angle_; - rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; - - std::mutex referee_mutex_; - std::deque referee_ring_buffer_receive_; - OutputInterface referee_serial_; - - int pub_time_count_ = 0; - int force_sensor_frame_count_ = 0; - int can2_unknown_count_ = 0; - - double first_sample_spot_ = 1.0; - double final_sample_spot_ = 4.0; - double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_; - std::vector time_samples_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV4, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp deleted file mode 100644 index 3d8d446d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/trigger_servo.hpp" - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware { - -// class DartFillingTestHardware -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::agent::CBoard { -// public: -// DartFillingTestHardware() -// : Node{ -// get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} -// , logger_{get_logger()} -// , dart_command_(create_partner_component(get_component_name() + "_command",*this)) -// , dr16_(*this) -// , limiting_servo_(dart_command_, "/dart/limiting_servo",LIMITING_SERVO_ID) -// , lifting_left_(*this, *this->dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) -// , lifting_right_(*this, *this->dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) -// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} -// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} -// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} -// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} -// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} -// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} -// , force_sensor_{*this} -// // , transmit_buffer_(*this, 32) -// // , event_thread_([this]() { handle_events(); }) -// { -// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); -// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - -// limiting_calibrate_subscription_ = create_subscription( -// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) -// { -// trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), -// limiting_servo_uart_data_ptr); -// }); -// lifting_left_calibrate_subscription_ = create_subscription( -// "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& -// msg) { -// trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), -// lifting_left_uart_data_ptr); -// }); -// lifting_right_calibrate_subscription_ = create_subscription( -// "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& -// msg) { -// trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), -// lifting_right_uart_data_ptr); -// }); - -// 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; -// // }; - -// last_read_left_time_ = this->now(); -// last_read_right_time_ = this->now(); -// } - -// ~DartFillingTestHardware() override { -// // stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); -// } - -// void command_update() { - -// if (!trigger_servo_.calibrate_mode()) { -// uint16_t current_target = trigger_servo_.get_target_angle(); -// if (current_target != last_trigger_angle_) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, -// uart_data_length); last_trigger_angle_ = current_target; -// } -// } - -// if (!limiting_servo_.calibrate_mode()) { -// uint16_t current_target = limiting_servo_.get_target_angle(); -// if (current_target != last_limiting_angle_) { -// size_t uart_data_length; -// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_limiting_angle_ = current_target; -// } -// } - -// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { -// uint16_t current_target_left = lifting_left_.get_target_angle(); -// uint16_t current_target_right = lifting_right_.get_target_angle(); -// if (current_target_left != last_lifting_left_angle_) { -// size_t uart_data_length; -// uint16_t runtime_left = 0; -// uint16_t runtime_right = 0; -// std::unique_ptr command_buffer = -// device::TriggerServo::generate_sync_run_command(uart_data_length, -// LIFTING_LEFT_ID, -// LIFTING_RIGHT_ID, -// current_target_left, -// current_target_right, -// runtime_left, runtime_right); -// const auto lifting_table_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, -// uart_data_length); last_lifting_left_angle_ = current_target_left; -// } -// } - -// auto now = this->now(); - -// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { -// size_t uart_data_length; -// auto command_buffer = lifting_left_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_left_time_ = now; -// } -// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) -// { -// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { -// size_t uart_data_length; -// auto command_buffer = lifting_right_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, -// uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_right_time_ = now; -// } -// } - -// transmit_buffer_.trigger_transmission(); -// } - -// int pub_time_count_ = 0; - -// protected: -// void uart1_receive_callback(const std::byte* data, uint8_t length) override { -// referee_ring_buffer_receive_.emplace_back_multi( -// [&data](std::byte* storage) { *storage = *data++; }, length); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// std::string hex_string = bytes_to_hex_string(data, length); -// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, -// hex_string.c_str()); - -// if (length < 3) { -// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); -// return; -// } - -// uint8_t servo_id = static_cast(data[2]); -// std::pair result{false, 0}; - -// switch (servo_id) { -// case TRIGGER_SERVO_ID: -// result = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIMITING_SERVO_ID: -// result = limiting_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_LEFT_ID: -// result = lifting_left_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_left_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_RIGHT_ID: -// result = lifting_right_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_right_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// break; -// } - -// } - -// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { -// dr16_.store_status(uart_data, uart_data_length); -// } - -// private: -// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; -// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; -// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; -// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; -// std::byte * trigger_servo_uart_data_ptr; -// std::byte * limiting_servo_uart_data_ptr; -// std::byte * lifting_left_uart_data_ptr; -// std::byte * lifting_right_uart_data_ptr; - -// void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ -// , std_msgs::msg::Int32::UniquePtr msg -// , std::byte* servo_uart_data_ptr) { -// /* -// 标定命令格式： -// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" -// 替换data值就行 -// */ -// servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - -// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); -// RCLCPP_INFO( -// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, -// hex_string.c_str()); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } // DEBUG TOOL - -// rclcpp::Logger logger_; -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(DartFillingTestHardware& robot) : dart_(robot) {} -// void update() override { dart_.command_update(); } -// DartFillingTestHardware& dart_; -// }; - -// std::shared_ptr dart_command_; -// device::Dr16 dr16_; -// device::TriggerServo limiting_servo_; -// device::TriggerServo lifting_left_; -// device::TriggerServo lifting_right_; -// device::DjiMotor left_belt_motor_, right_belt_motor_; -// device::DjiMotor yaw_control_motor_, pitch_control_motor_; -// device::DjiMotor screw_motor_; -// device::PWMServo trigger_servo_; -// device::ForceSensor force_sensor_; -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; -// OutputInterface referee_serial_; -// std::thread event_thread_; -// OutputInterface lifting_current_angle_left_; -// OutputInterface lifting_current_angle_right_; -// std::vector yaw_samples_, time_samples_; -// uint16_t last_trigger_angle_ = 0xFFFF; -// uint16_t last_limiting_angle_ = 0xFFFF; -// uint16_t last_lifting_left_angle_ = 0xFFFF; -// rclcpp::Time last_read_left_time_; -// rclcpp::Time last_read_right_time_; -// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -// }; -// } // namespace rmcs_core::hardware - -// #include -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DartFillingTestHardware, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index d09af3dd..ec6f8bdf 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit d09af3dd7cebf2b1af4067a938f1b89d1824834a +Subproject commit ec6f8bdf60ad05e10f235f15ccbef08e84b670db From 685bb424069b7131f5a92d33bd3150a1aeb16686 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Tue, 17 Mar 2026 00:46:18 +0800 Subject: [PATCH 39/45] launch process with two visions --- .../config/dart-filling-test.yaml | 37 +- .../rmcs_bringup/config/dart-guidance-v2.yaml | 8 +- .../rmcs_bringup/config/dart-guidance.yaml | 4 +- rmcs_ws/src/rmcs_core/librmcs | 1 - rmcs_ws/src/rmcs_core/plugins.xml | 6 + .../src/controller/dart/dart_filling.cpp | 60 +++ .../dart/launch_controller_v2_full.cpp | 359 ------------------ .../src/controller/dart/launch_setting_v2.cpp | 42 +- .../hardware/catapult_dart_filling_test.cpp | 160 ++++++++ .../src/hardware/catapult_dart_librmcs_v3.cpp | 142 ------- rmcs_ws/src/rmcs_dart_guidance | 2 +- 11 files changed, 289 insertions(+), 532 deletions(-) delete mode 160000 rmcs_ws/src/rmcs_core/librmcs create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml index 17f481b7..8f3641c8 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml @@ -2,23 +2,32 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::CatapultDartV3Lk -> catapult_dart - - rmcs_core::controller::dart::DartFillingTest -> launch_controller + - rmcs_core::hardware::CatapultDartFillingTest -> dart_hardware + - rmcs_dart_guidance::manager::DartManagerForFillingTest -> dart_manager + - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + - rmcs_core::controller::dart::DartFilling -> dart_controller -catapult_dart: +dart_hardware: ros__parameters: serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 -launch_controller: +remote_cmd_bridge: ros__parameters: - # Trigger servo: PWM value 0.0–1.0 - # PWMServo range: 0.5ms–2.5ms in a 20ms period - # 0.0 → 0.5ms (full CCW), 0.5 → 1.5ms (center), 1.0 → 2.5ms (full CW) - trigger_free_value: 0.75 # tune to match physical free position - trigger_lock_value: 0.63 # tune to match physical lock position + joystick_sensitivity: 0.01 + +dart_manager: + ros__parameters: + # 升降堵转检测参数 + lifting_stall_threshold: 0.01 # rad/s，低于此值视为堵转 + lifting_stall_confirm_ticks: 10 # 连续帧数 = 0.1s @ 1kHz + lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 + lifting_stall_timeout_ticks: 5000 # 超时帧数 = 5s @ 1kHz + + # 限位舵机参数 + limiting_open_angle: 0xECB # uint16_t，舵机打开角度（需实测标定） + limiting_close_angle: 0xB53 # uint16_t，舵机关闭角度（需实测标定） + limiting_fill_ticks: 200 # 填装等待时间 = 0.5s @ 1kHz - # Motor torque limits (N·m) - max_torque: 1.0 - max_belt_torque: 5.0 +dart_controller: + ros__parameters: + lifting_velocity: 3.0 # rad/s，升降电机速度（正值 = UP时左+右-） diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml index 6f654128..765b94d4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml @@ -19,7 +19,7 @@ dart_hardware: dart_manager: ros__parameters: - max_transform_rate: 15.0 + max_transform_rate: 5.0 manual_force_scale: 5.0 limiting_open_angle: 500 # uint16_t，需实测标定 limiting_close_angle: 1000 # uint16_t，需实测标定 @@ -27,7 +27,7 @@ dart_manager: lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 - lifting_stall_timeout_ticks: 5000 # 超时帧数 = 5s + lifting_stall_timeout_ticks: 2000 # 超时帧数 = 5s remote_cmd_bridge: ros__parameters: @@ -35,9 +35,9 @@ remote_cmd_bridge: launch_setting: ros__parameters: - belt_velocity: 15.0 + belt_velocity: 10.0 sync_coefficient: 0.1 - max_control_torque: 15.0 + max_control_torque: 2.5 trigger_free_value: 0.75 trigger_lock_value: 0.63 b_kp: 5.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 486c1526..0bf98d8c 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -29,10 +29,10 @@ launch_setting: ros__parameters: belt_velocity: 15.0 sync_coefficient: 0.1 - max_control_torque: 15.0 + max_control_torque: 2.5 trigger_free_value: 0.75 trigger_lock_value: 0.63 - b_kp: 2.0 + b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs deleted file mode 160000 index eff14b71..00000000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eff14b71e334baaaa462a83c7e6d834886b10168 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 727accdf..2c350ee4 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -155,4 +155,10 @@ Launch setting V2: belt + trigger + LK lifting angle control. + + Launch setting V2: belt + trigger + LK lifting angle control. + + + Launch setting V2: belt + trigger + LK lifting angle control. + \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp new file mode 100644 index 00000000..ec863522 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp @@ -0,0 +1,60 @@ +#include + +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::dart { + +class DartFilling + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartFilling() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + + lifting_velocity_ = get_parameter("lifting_velocity").as_double(); + + register_input("/dart/manager/lifting/command", lifting_command_); + + register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); + register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); + } + + void update() override { + switch (*lifting_command_) { + case rmcs_msgs::DartSliderStatus::UP: + *lifting_left_vel_ = -lifting_velocity_; + *lifting_right_vel_ = +lifting_velocity_; + break; + case rmcs_msgs::DartSliderStatus::DOWN: + *lifting_left_vel_ = +lifting_velocity_; + *lifting_right_vel_ = -lifting_velocity_; + break; + default: + *lifting_left_vel_ = 0.0; + *lifting_right_vel_ = 0.0; + break; + } + } + +private: + rclcpp::Logger logger_; + + double lifting_velocity_; + + InputInterface lifting_command_; + OutputInterface lifting_left_vel_; + OutputInterface lifting_right_vel_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::dart::DartFilling, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp deleted file mode 100644 index 490c5f9f..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_controller_v2_full.cpp +++ /dev/null @@ -1,359 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -DartLaunchControllerV2Full — 发射控制 Phase 2（含升降电机与限位舵机） -升降电机：瓴控4005 (LkMotor)，control_angle 接口为 double (rad)。 - yaml 参数用度数，代码内自动换算弧度。 -限位舵机：TriggerServo UART，control_angle 接口为 uint16_t。 - -完整发射流程状态机： - DISABLE → 左中 → RESETTING - RESETTING（带反转）→ 堵转 → INIT（带停，trigger LOCK） - INIT + 右中→下 → LOADING（带正转） - LOADING（带正转）→ 堵转 → 降下升降平台 + 延时500ms - → RESETTING（trigger LOCK）+ loading_process（限位舵机开→延时2s→关） - RESETTING（trigger LOCK）→ 堵转 → READY（带停，trigger LOCK） - READY + 右中→上 → 发射：trigger FREE + 升起升降平台 - → 延时后 trigger LOCK → RESETTING（下一轮） - READY + 右中→下 → CANCEL：升起升降平台 → RESETTING（trigger LOCK=false） -*/ - -namespace rmcs_core::controller::dart { - -class DartLaunchControllerV2Full - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchControllerV2Full() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , timer_interval_ms_(10) - , logger_(get_logger()) { - - drive_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - launch_trigger_value_ = get_parameter("trigger_free_value").as_double(); - launch_lock_value_ = get_parameter("trigger_lock_value").as_double(); - trigger_free_duration_ms_ = get_parameter("trigger_free_duration_ms").as_int(); - lifting_settle_ms_ = get_parameter("lifting_settle_ms").as_int(); - limiting_open_duration_ms_ = get_parameter("limiting_open_duration_ms").as_int(); - - // Lifting angles: yaml in degrees, stored as radians - constexpr double kDegToRad = std::numbers::pi / 180.0; - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left_deg").as_double() - * kDegToRad; - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left_deg").as_double() - * kDegToRad; - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right_deg").as_double() - * kDegToRad; - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right_deg").as_double() - * kDegToRad; - - // Limiting servo angles (UART TriggerServo, uint16_t hex values) - limiting_free_angle_ = static_cast( - get_parameter("limiting_free_angle_").as_int()); - limiting_lock_angle_ = static_cast( - get_parameter("limiting_lock_angle_").as_int()); - - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - - // Lifting angle feedback from LkMotor (double, rad) - register_input("/dart/lifting_left/angle", lifting_angle_left_); - register_input("/dart/lifting_right/angle", lifting_angle_right_); - - register_output( - "/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0.0); - register_output( - "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0.0); - register_output("/dart/trigger_servo/value", trigger_value_, launch_lock_value_); - - // Lifting control: double (rad) → LkMotor control_angle - // Start with NaN to disable motors until explicitly commanded - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle_, - static_cast(std::numeric_limits::quiet_NaN())); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle_, - static_cast(std::numeric_limits::quiet_NaN())); - - // Limiting servo: uint16_t → TriggerServo control_angle, start locked - register_output("/dart/limiting_servo/control_angle", limiting_control_angle_, - limiting_lock_angle_); - - register_output("/dart/filling/stage", filling_stage_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms_), - [this] { timer_callback(); }); - } - - void update() override { - using namespace rmcs_msgs; - - auto sw_left = *switch_left_; - auto sw_right = *switch_right_; - - const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) - && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); - - if (all_down) { - reset_all_controls(); - // SAFETY: trigger LOCK immediately, no dependency on flag - *trigger_value_ = launch_lock_value_; - - } else if (sw_left == Switch::MIDDLE) { - - // Transition out of DISABLE - if (stage_ == DartLaunchStages::DISABLE) { - stage_ = DartLaunchStages::RESETTING; - drive_belt_block_count_ = 0; - // Raise platform at startup - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - *filling_stage_ = DartFillingStages::LIFTING; - RCLCPP_INFO(logger_, "DISABLE → RESETTING, lifting UP"); - } - - // Switch edge: right MIDDLE → DOWN - if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { - if (stage_ == DartLaunchStages::INIT) { - stage_ = DartLaunchStages::LOADING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "INIT → LOADING"); - } else if (stage_ == DartLaunchStages::READY) { - // Cancel: lift platform back up, keep trigger LOCKED during CANCEL/RESETTING - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - *filling_stage_ = DartFillingStages::LIFTING; - stage_ = DartLaunchStages::CANCEL; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "READY → CANCEL, lifting UP, trigger stays LOCKED"); - } - - // Switch edge: right MIDDLE → UP - } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { - if (stage_ == DartLaunchStages::READY) { - // Launch! - trigger_lock_flag_ = false; - stage_ = DartLaunchStages::INIT; // stop belt during fire - *filling_stage_ = DartFillingStages::INIT; - // Raise platform simultaneously - *lifting_left_control_angle_ = lifting_up_angle_left_; - *lifting_right_control_angle_ = lifting_up_angle_right_; - RCLCPP_INFO(logger_, "READY → LAUNCH (trigger free, lifting UP)"); - delay_and_execute(trigger_free_duration_ms_, [this]() { - trigger_lock_flag_ = true; - stage_ = DartLaunchStages::RESETTING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "LAUNCH → RESETTING"); - }); - } else { - RCLCPP_WARN(logger_, "Cannot launch: not in READY state (current: %d)", - static_cast(stage_)); - } - } - - // Blocking detection → automatic stage transitions - if (blocking_detection()) { - if (stage_ == DartLaunchStages::RESETTING) { - // trigger_lock_flag_ determines next state - if (trigger_lock_flag_) { - stage_ = DartLaunchStages::READY; - *filling_stage_ = DartFillingStages::READY; - RCLCPP_INFO(logger_, "RESETTING → READY (blocked, lock=true)"); - } else { - stage_ = DartLaunchStages::INIT; - *filling_stage_ = DartFillingStages::INIT; - RCLCPP_INFO(logger_, "RESETTING → INIT (blocked, lock=false)"); - } - drive_belt_block_count_ = 0; - - } else if (stage_ == DartLaunchStages::LOADING) { - // Lower the lifting platform, then start resetting - *lifting_left_control_angle_ = lifting_down_angle_left_; - *lifting_right_control_angle_ = lifting_down_angle_right_; - *filling_stage_ = DartFillingStages::DOWNING; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "LOADING blocked → lifting DOWN"); - delay_and_execute(lifting_settle_ms_, [this]() { - stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = true; - drive_belt_block_count_ = 0; - RCLCPP_INFO(logger_, "Lifting settled → RESETTING (lock=true)"); - // Open limiting servo to let dart feed, then close - loading_process(); - }); - - } else if (stage_ == DartLaunchStages::CANCEL) { - // Platform fully raised; RESETTING will decompress spring. - // trigger_lock_flag_ = false so RESETTING → INIT (not READY) - stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = false; - drive_belt_block_count_ = 0; - *filling_stage_ = DartFillingStages::INIT; - RCLCPP_INFO(logger_, "CANCEL → RESETTING (lock=false → INIT)"); - } - } - - // Belt velocity based on current stage - double control_velocity = 0.0; - if (stage_ == DartLaunchStages::RESETTING) { - control_velocity = -drive_belt_working_velocity_; - } else if (stage_ == DartLaunchStages::LOADING - || stage_ == DartLaunchStages::CANCEL) { - control_velocity = drive_belt_working_velocity_; - } - drive_belt_sync_control(control_velocity); - - // Trigger servo: updated only in MIDDLE mode - *trigger_value_ = trigger_lock_flag_ ? launch_lock_value_ : launch_trigger_value_; - } - // Note: in LEFT_UP (DartSettings) mode, trigger maintains its last value - - last_switch_left_ = sw_left; - last_switch_right_ = sw_right; - } - -private: - void reset_all_controls() { - stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - *left_drive_belt_control_torque_ = 0.0; - *right_drive_belt_control_torque_ = 0.0; - // Cancel any pending delayed action (e.g. mid-launch or mid-loading timer) - is_delaying_ = false; - delayed_action_ = nullptr; - trigger_lock_flag_ = false; - drive_belt_block_count_ = 0; - *limiting_control_angle_ = limiting_lock_angle_; - // Trigger LOCK is set directly in the all_down branch of update() - } - - void loading_process() { - // Open limiting servo to let dart drop into position - *limiting_control_angle_ = limiting_free_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::FILLING; - RCLCPP_INFO(logger_, "loading_process: limiting servo OPEN"); - delay_and_execute(limiting_open_duration_ms_, [this]() { - *limiting_control_angle_ = limiting_lock_angle_; - *filling_stage_ = rmcs_msgs::DartFillingStages::READY; - RCLCPP_INFO(logger_, "loading_process: limiting servo LOCK"); - }); - } - - void drive_belt_sync_control(double set_velocity) { - if (set_velocity == 0.0) { - *left_drive_belt_control_torque_ = 0.0; - *right_drive_belt_control_torque_ = 0.0; - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_drive_belt_velocity_, - set_velocity - *right_drive_belt_velocity_}; - Eigen::Vector2d relative_velocity{ - *left_drive_belt_velocity_ - *right_drive_belt_velocity_, - *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - - *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); - } - - bool blocking_detection() { - if ((std::abs(*left_drive_belt_velocity_) < 0.5 - && std::abs(*left_drive_belt_control_torque_) > 0.5) - || (std::abs(*right_drive_belt_velocity_) < 0.5 - && std::abs(*right_drive_belt_control_torque_) > 0.5)) { - drive_belt_block_count_++; - } - return drive_belt_block_count_ > 1000; - } - - rclcpp::TimerBase::SharedPtr timer_; - int timer_interval_ms_; - std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; - - void timer_callback() { - if (is_delaying_ && delay_remaining_ms_ > 0) { - delay_remaining_ms_ -= timer_interval_ms_; - if (delay_remaining_ms_ <= 0) { - is_delaying_ = false; - if (delayed_action_) - delayed_action_(); - } - } - } - - void delay_and_execute(int delay_ms, std::function action) { - if (!is_delaying_) { - is_delaying_ = true; - delay_remaining_ms_ = delay_ms; - delayed_action_ = std::move(action); - } - } - - rclcpp::Logger logger_; - - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - - InputInterface left_drive_belt_velocity_; - InputInterface right_drive_belt_velocity_; - OutputInterface left_drive_belt_control_torque_; - OutputInterface right_drive_belt_control_torque_; - - double drive_belt_working_velocity_; - double sync_coefficient_; - double max_control_torque_; - int drive_belt_block_count_ = 0; - - double launch_trigger_value_; - double launch_lock_value_; - int trigger_free_duration_ms_; - bool trigger_lock_flag_ = false; - OutputInterface trigger_value_; - - // Lifting motors (LK4005): double angle in radians - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - OutputInterface lifting_left_control_angle_; - OutputInterface lifting_right_control_angle_; - double lifting_up_angle_left_; - double lifting_down_angle_left_; - double lifting_up_angle_right_; - double lifting_down_angle_right_; - int lifting_settle_ms_; - - // Limiting servo (TriggerServo UART): uint16_t - OutputInterface limiting_control_angle_; - uint16_t limiting_free_angle_; - uint16_t limiting_lock_angle_; - int limiting_open_duration_ms_; - - rmcs_msgs::DartLaunchStages stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - OutputInterface filling_stage_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchControllerV2Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp index 7ab4e08b..7309e59f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp @@ -51,8 +51,10 @@ class DartLaunchSettingV2 register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); register_input("/dart/manager/lifting/command", lifting_command_); - // 第一发下降速度缩放因子（shot_count==1 时 0.5，其余 1.0） + // 第一发下降速度缩放因子（shot_count==1 时 0.8，其余 1.0） register_input("/dart/manager/belt/down_scale", belt_down_scale_); + // 归零模式：true 时将传送带扭矩上限限制到 10%，防止顶住机械限位过热 + register_input("/dart/manager/belt/homing", belt_homing_mode_); register_output( "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); @@ -78,23 +80,46 @@ class DartLaunchSettingV2 control_velocity = 0.0; break; } - drive_belt_sync_control(control_velocity); + double torque_limit = *belt_homing_mode_ + ? max_control_torque_ * 0.1 + : max_control_torque_; + drive_belt_sync_control(control_velocity, torque_limit); *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); + + // 升降电机速度控制（UP/DOWN/WAIT → 速度映射） + switch (*lifting_command_) { + case rmcs_msgs::DartSliderStatus::UP: + *lifting_left_vel_ = +lifting_velocity_; + *lifting_right_vel_ = -lifting_velocity_; + break; + case rmcs_msgs::DartSliderStatus::DOWN: + *lifting_left_vel_ = -lifting_velocity_; + *lifting_right_vel_ = +lifting_velocity_; + break; + default: + *lifting_left_vel_ = 0.0; + *lifting_right_vel_ = 0.0; + break; + } } private: - void drive_belt_sync_control(double set_velocity) { - // WAIT 状态：清除 PID 积分；若上一个方向是 DOWN，施加保持扭矩防止滑落 + void drive_belt_sync_control(double set_velocity, double torque_limit) { + // WAIT 状态：清除 PID 积分，避免残余扭矩 + // 上端（prev=UP）：完全清零，避免顶着限位持续出力 + // 下端（prev=DOWN）：施加保持扭矩，防止重力/弹力导致滑台回弹 if (set_velocity == 0.0) { belt_pid_.reset(); if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { *left_belt_torque_ = +belt_hold_torque_; *right_belt_torque_ = +belt_hold_torque_; + RCLCPP_INFO(logger_, "belt_combating"); } else { *left_belt_torque_ = 0.0; *right_belt_torque_ = 0.0; + RCLCPP_INFO(logger_, "belt_init done"); } return; } @@ -109,10 +134,8 @@ class DartLaunchSettingV2 Eigen::Vector2d control_torques = belt_pid_.update(setpoint_error) - sync_coefficient_ * relative_velocity; - *left_belt_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_belt_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + *left_belt_torque_ = std::clamp(control_torques[0], -torque_limit, torque_limit); + *right_belt_torque_ = std::clamp(control_torques[1], -torque_limit, torque_limit); } rclcpp::Logger logger_; @@ -137,6 +160,7 @@ class DartLaunchSettingV2 InputInterface lifting_command_; InputInterface belt_down_scale_; + InputInterface belt_homing_mode_; OutputInterface left_belt_torque_; OutputInterface right_belt_torque_; @@ -149,4 +173,4 @@ class DartLaunchSettingV2 #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) + rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp new file mode 100644 index 00000000..1d78925f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp @@ -0,0 +1,160 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/can_packet.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/trigger_servo.hpp" +#include "librmcs/agent/c_board.hpp" + +namespace rmcs_core::hardware { + +class CatapultDartFillingTest + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + CatapultDartFillingTest() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} + , dart_command_( + create_partner_component(get_component_name() + "_command", *this)) + , logger_{get_logger()} + , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} + , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} + , limiting_servo_{*dart_command_, "/dart/limiting_servo", 0x04} + + , dr16_{*this} { + lifting_left_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + lifting_right_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} + .enable_multi_turn_angle()); + } + + CatapultDartFillingTest(const CatapultDartFillingTest&) = delete; + CatapultDartFillingTest& operator=(const CatapultDartFillingTest&) = delete; + CatapultDartFillingTest(CatapultDartFillingTest&&) = delete; + CatapultDartFillingTest& operator=(CatapultDartFillingTest&&) = delete; + + ~CatapultDartFillingTest() override = default; + + void update() override { + dr16_.update_status(); + lifting_left_motor_.update_status(); + lifting_right_motor_.update_status(); + } + + void command_update() { + auto board = start_transmit(); + + board.can1_transmit({ + .can_id = 0x141, + .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), + }); + + board.can1_transmit({ + .can_id = 0x145, + .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), + }); + + if (!limiting_servo_.calibrate_mode()) { + uint16_t current_target = limiting_servo_.get_target_angle(); + if (current_target != last_limiting_angle_) { + size_t uart_data_length; + auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); + board.uart2_transmit( + {.uart_data = std::span{command_buffer.get(), uart_data_length}}); + last_limiting_angle_ = current_target; + RCLCPP_INFO(logger_, "limiting_servo angle: 0x%04X", current_target); + } + } + } + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + std::lock_guard lock(referee_mutex_); + for (auto byte : data.uart_data) { + referee_ring_buffer_receive_.push_back(byte); + } + } + + void uart2_receive_callback(const librmcs::data::UartDataView& data) override { + + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + +private: + static std::string bytes_to_hex_string(const std::byte* data, size_t size) { + if (!data || size == 0) + return "[]"; + std::stringstream ss; + ss << std::hex << std::uppercase << std::setfill('0'); + for (size_t i = 0; i < size; ++i) + ss << std::setw(2) << static_cast(data[i]) << " "; + std::string result = ss.str(); + if (!result.empty() && result.back() == ' ') + result.pop_back(); + return result; + } + + class DartCommand : public rmcs_executor::Component { + public: + explicit DartCommand(CatapultDartFillingTest& dart) + : dart_(dart) {} + void update() override { dart_.command_update();} + + private: + CatapultDartFillingTest& dart_; + }; + + std::shared_ptr dart_command_; + rclcpp::Logger logger_; + + uint16_t last_limiting_angle_{0xFFFF}; + + device::LkMotor lifting_left_motor_; + device::LkMotor lifting_right_motor_; + + device::TriggerServo limiting_servo_; + + device::Dr16 dr16_; + + std::mutex referee_mutex_; + std::deque referee_ring_buffer_receive_; + OutputInterface referee_serial_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartFillingTest, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp deleted file mode 100644 index 9c6eb2ae..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_librmcs_v3.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" - -// namespace rmcs_core::hardware { - -// class CatapultDartforlibrmcsv3 -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::agent::CBoard { -// public: -// CatapultDartforlibrmcsv3() -// : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} -// , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , logger_{get_logger()} -// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} -// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} -// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} -// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} -// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} -// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} -// , force_sensor_{*this} { - -// left_belt_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// right_belt_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508} -// .set_reversed() -// .set_reduction_ratio(19.)); - -// yaw_control_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// pitch_control_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - -// screw_motor_.configure( -// device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); -// } - -// CatapultDartforlibrmcsv3(const CatapultDartforlibrmcsv3&) = delete; -// CatapultDartforlibrmcsv3& operator=(const CatapultDartforlibrmcsv3&) = delete; -// CatapultDartforlibrmcsv3(CatapultDartforlibrmcsv3&&) = delete; -// CatapultDartforlibrmcsv3& operator=(CatapultDartforlibrmcsv3&&) = delete; - -// ~CatapultDartforlibrmcsv3() override = default; - -// void update() override { force_sensor_.update_status(); } - -// void command_update() { -// auto board = start_transmit(); -// board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - -// board.can2_transmit({ -// .can_id = 0x200, -// .can_data = -// device::CanPacket8{ -// pitch_control_motor_.generate_command(), -// yaw_control_motor_.generate_command(), -// screw_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// board.can2_transmit({ -// .can_id = 0x1FF, -// .can_data = -// device::CanPacket8{ -// left_belt_motor_.generate_command(), -// right_belt_motor_.generate_command(), -// device::CanPacket8::PaddingQuarter{}, -// device::CanPacket8::PaddingQuarter{}, -// } -// .as_bytes(), -// }); - -// if (pub_time_count_++ > 100) { -// board.can2_transmit({ -// .can_id = 0x301, -// .can_data = device::CanPacket8{device::ForceSensor::generate_command()}.as_bytes(), -// }); -// pub_time_count_ = 0; -// } -// } - -// protected: -// void can2_receive_callback(const librmcs::data::CanDataView& data) override { -// if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] -// return; - -// auto can_id = data.can_id; -// if (can_id == 0x201) { -// pitch_control_motor_.store_status(data.can_data); -// } else if (can_id == 0x202) { -// yaw_control_motor_.store_status(data.can_data); -// } else if (can_id == 0x203) { -// screw_motor_.store_status(data.can_data); -// } else if (can_id == 0x205) { -// left_belt_motor_.store_status(data.can_data); -// } else if (can_id == 0x206) { -// right_belt_motor_.store_status(data.can_data); -// } else if (can_id == 0x302) { -// force_sensor_.store_status(data.can_data); -// } -// } - -// private: -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDartforlibrmcsv3& dart) -// : dart_(dart) {} - -// void update() override { dart_.command_update(); } - -// private: -// CatapultDartforlibrmcsv3& dart_; -// }; -// std::shared_ptr dart_command_; - -// rclcpp::Logger logger_; - -// device::DjiMotor left_belt_motor_, right_belt_motor_; -// device::DjiMotor yaw_control_motor_, pitch_control_motor_; -// device::DjiMotor screw_motor_; -// device::PWMServo trigger_servo_; -// device::ForceSensor force_sensor_; -// int pub_time_count_ = 0; -// }; -// } // namespace rmcs_core::hardware - -// #include - -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartforlibrmcsv3, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index d09af3dd..db6b7f20 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit d09af3dd7cebf2b1af4067a938f1b89d1824834a +Subproject commit db6b7f20d52313c23cb4bd64446c8281d5137b39 From d709d92c11afb24a0c5209c78460187ef7989863 Mon Sep 17 00:00:00 2001 From: Ubuntu <13869662005@163.com> Date: Tue, 17 Mar 2026 00:51:47 +0800 Subject: [PATCH 40/45] launch process with filling --- .../config/dart-filling-test.yaml | 33 - .../rmcs_bringup/config/dart-guidance-v2.yaml | 71 --- .../rmcs_bringup/config/dart-guidance.yaml | 34 +- .../rmcs_bringup/config/dart-launch-full.yaml | 74 --- .../config/dart-remot-bridge-test.yaml | 16 - rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml | 14 - .../src/controller/dart/dart_filling.cpp | 60 -- .../src/controller/dart/dart_filling_test.cpp | 206 ------- .../src/controller/dart/debug_info.cpp | 272 --------- .../src/controller/dart/launch_setting.cpp | 95 ++- .../src/controller/dart/launch_setting_v2.cpp | 176 ------ .../src/controller/dart/pwm_test.cpp | 28 - .../src/controller/dart/servo_test.cpp | 97 --- .../dartlauncher/dart_setttings.cpp | 109 ---- .../dartlauncher/launch_controller.cpp | 209 ------- .../rmcs_core/src/hardware/catapult_dart.cpp | 567 ------------------ .../hardware/catapult_dart_filling_test.cpp | 160 ----- .../src/hardware/catapult_dart_v3_full.cpp | 426 ------------- .../src/hardware/catapult_dart_v4.cpp | 449 -------------- .../hardware/dart_filling_test_hardware.cpp | 337 ----------- 20 files changed, 103 insertions(+), 3330 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml delete mode 100644 rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml deleted file mode 100644 index 8f3641c8..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-filling-test.yaml +++ /dev/null @@ -1,33 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartFillingTest -> dart_hardware - - rmcs_dart_guidance::manager::DartManagerForFillingTest -> dart_manager - - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge - - rmcs_core::controller::dart::DartFilling -> dart_controller - -dart_hardware: - ros__parameters: - serial_filter: "" - -remote_cmd_bridge: - ros__parameters: - joystick_sensitivity: 0.01 - -dart_manager: - ros__parameters: - # 升降堵转检测参数 - lifting_stall_threshold: 0.01 # rad/s，低于此值视为堵转 - lifting_stall_confirm_ticks: 10 # 连续帧数 = 0.1s @ 1kHz - lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 - lifting_stall_timeout_ticks: 5000 # 超时帧数 = 5s @ 1kHz - - # 限位舵机参数 - limiting_open_angle: 0xECB # uint16_t，舵机打开角度（需实测标定） - limiting_close_angle: 0xB53 # uint16_t，舵机关闭角度（需实测标定） - limiting_fill_ticks: 200 # 填装等待时间 = 0.5s @ 1kHz - -dart_controller: - ros__parameters: - lifting_velocity: 3.0 # rad/s，升降电机速度（正值 = UP时左+右-） diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml deleted file mode 100644 index 765b94d4..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance-v2.yaml +++ /dev/null @@ -1,71 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware - - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager - - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge - # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge - - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting - - rmcs_core::controller::dart::DartSettingController -> dart_setting - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -dart_manager: - ros__parameters: - max_transform_rate: 5.0 - manual_force_scale: 5.0 - limiting_open_angle: 500 # uint16_t，需实测标定 - limiting_close_angle: 1000 # uint16_t，需实测标定 - limiting_fill_ticks: 500 - lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 - lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz - lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 - lifting_stall_timeout_ticks: 2000 # 超时帧数 = 5s - -remote_cmd_bridge: - ros__parameters: - joystick_sensitivity: 0.01 - -launch_setting: - ros__parameters: - belt_velocity: 10.0 - sync_coefficient: 0.1 - max_control_torque: 2.5 - trigger_free_value: 0.75 - trigger_lock_value: 0.63 - b_kp: 5.0 - b_ki: 0.0 - b_kd: 0.0 - lifting_velocity: 10.0 # rad/s，正值 = 左+右-（平台上升方向），需实测标定方向 - belt_hold_torque: 15.0 # Nm，传送带下方堵转后的保持扭矩，需实测标定 - -dart_setting: - ros__parameters: - yaw_kp: 1.0 - yaw_ki: 0.0 - yaw_kd: 0.0 - pitch_kp: 1.0 - pitch_ki: 0.0 - pitch_kd: 0.0 - -force_screw_velocity_pid_controller: - ros__parameters: - measurement: /dart/force_screw_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_screw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 - -broadcaster: - ros__parameters: - forward_list: - - /dart/drive_belt/left/control_torque - - /dart/drive_belt/right/control_torque \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 0bf98d8c..765b94d4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -2,13 +2,14 @@ rmcs_executor: ros__parameters: update_rate: 1000.0 components: - - rmcs_core::hardware::CatapultDartV4 -> dart_hardware - - rmcs_dart_guidance::manager::DartManager -> dart_manager + - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware + - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge - - rmcs_core::controller::dart::DartLaunchSetting -> launch_setting + - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting - rmcs_core::controller::dart::DartSettingController -> dart_setting - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster dart_hardware: ros__parameters: @@ -18,8 +19,15 @@ dart_hardware: dart_manager: ros__parameters: - max_transform_rate: 15.0 + max_transform_rate: 5.0 manual_force_scale: 5.0 + limiting_open_angle: 500 # uint16_t，需实测标定 + limiting_close_angle: 1000 # uint16_t，需实测标定 + limiting_fill_ticks: 500 + lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 + lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz + lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 + lifting_stall_timeout_ticks: 2000 # 超时帧数 = 5s remote_cmd_bridge: ros__parameters: @@ -27,14 +35,16 @@ remote_cmd_bridge: launch_setting: ros__parameters: - belt_velocity: 15.0 + belt_velocity: 10.0 sync_coefficient: 0.1 max_control_torque: 2.5 trigger_free_value: 0.75 trigger_lock_value: 0.63 - b_kp: 1.0 + b_kp: 5.0 b_ki: 0.0 b_kd: 0.0 + lifting_velocity: 10.0 # rad/s，正值 = 左+右-（平台上升方向），需实测标定方向 + belt_hold_torque: 15.0 # Nm，传送带下方堵转后的保持扭矩，需实测标定 dart_setting: ros__parameters: @@ -47,9 +57,15 @@ dart_setting: force_screw_velocity_pid_controller: ros__parameters: - measurement: /dart/force_screw/velocity + measurement: /dart/force_screw_motor/velocity setpoint: /force/control/velocity - control: /dart/force_screw/control_torque + control: /dart/force_screw_motor/control_torque kp: 1.0 ki: 0.0 kd: 0.0 + +broadcaster: + ros__parameters: + forward_list: + - /dart/drive_belt/left/control_torque + - /dart/drive_belt/right/control_torque \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml deleted file mode 100644 index bf3959f6..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-launch-full.yaml +++ /dev/null @@ -1,74 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware - - rmcs_core::controller::dart::DartLaunchControllerV2Full -> launch_controller - - rmcs_core::controller::dart::DartSettings -> dart_settings - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller - - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster - -broadcaster: - ros__parameters: - forward_list: - - /dart/drive_belt/left/control_torque - - /dart/drive_belt/left/velocity - - /dart/drive_belt/right/control_torque - - /dart/drive_belt/right/velocity - - /dart/lifting_left/angle - - /dart/lifting_right/angle - - /force/sensor/average - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 - -launch_controller: - ros__parameters: - belt_velocity: 10.0 - sync_coefficient: 0.1 - max_control_torque: 15.0 - # Trigger servo PWM value 0.0–1.0 (from dart-filling-test calibration) - trigger_free_value: 0.75 - trigger_lock_value: 0.66 - # Duration to keep trigger free after launch (ms) - trigger_free_duration_ms: 300 - # Time to wait after platform move before belt resets (ms) - lifting_settle_ms: 500 - # Duration to keep limiting servo open for dart feeding (ms) - limiting_open_duration_ms: 2000 - # Lifting motor angles in DEGREES — will be converted to radians in code. - # 待标定: 上电后将平台移到 up 位置，记录角度填入 *_up_deg； - # 移到 down 位置记录填入 *_down_deg。 - lifting_up_angle_left_deg: 0.0 - lifting_down_angle_left_deg: 90.0 - lifting_up_angle_right_deg: 0.0 - lifting_down_angle_right_deg: 90.0 - # Limiting servo (TriggerServo UART, uint16_t hex) - limiting_free_angle_: 0x0001 - limiting_lock_angle_: 0x0050 - -dart_settings: - ros__parameters: - is_auto_pitch_control_mode: 0 - is_auto_force_control_mode: 0 - max_transform_rate: 5.0 - pitch_angle_kp: 1.0 - pitch_angle_ki: 0.0 - pitch_angle_kd: 0.0 - force_control_kp: 1.0 - force_control_ki: 0.0 - force_control_kd: 0.0 - -force_screw_velocity_pid_controller: - ros__parameters: - measurement: /dart/force_screw_motor/velocity - setpoint: /force/control/velocity - control: /dart/force_screw_motor/control_torque - kp: 1.0 - ki: 0.0 - kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml deleted file mode 100644 index d0ed8157..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/dart-remot-bridge-test.yaml +++ /dev/null @@ -1,16 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge - - rmcs_core::hardware::CatapultDartV4 -> dart_hardware - -remote_cmd_bridge: - ros__parameters: - joystick_sensitivity: 0.01 - -dart_hardware: - ros__parameters: - serial_filter: "" - first_sample_spot: 1.0 - final_sample_spot: 4.0 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml b/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml deleted file mode 100644 index 2a1c3b81..00000000 --- a/rmcs_ws/src/rmcs_bringup/config/pwm_test.yaml +++ /dev/null @@ -1,14 +0,0 @@ -rmcs_executor: - ros__parameters: - update_rate: 1000.0 - components: - - rmcs_core::hardware::CatapultDartforlibrmcsv3 -> catapult_dart_hardware - - rmcs_core::controller::test::PWMTest -> pwm_test_controller - -catapult_dart_hardware: - ros__parameters: - serial_filter: "" - -pwm_test_controller: - ros__parameters: - set_value: 0.5 \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp deleted file mode 100644 index ec863522..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::controller::dart { - -class DartFilling - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartFilling() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - - lifting_velocity_ = get_parameter("lifting_velocity").as_double(); - - register_input("/dart/manager/lifting/command", lifting_command_); - - register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); - register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); - } - - void update() override { - switch (*lifting_command_) { - case rmcs_msgs::DartSliderStatus::UP: - *lifting_left_vel_ = -lifting_velocity_; - *lifting_right_vel_ = +lifting_velocity_; - break; - case rmcs_msgs::DartSliderStatus::DOWN: - *lifting_left_vel_ = +lifting_velocity_; - *lifting_right_vel_ = -lifting_velocity_; - break; - default: - *lifting_left_vel_ = 0.0; - *lifting_right_vel_ = 0.0; - break; - } - } - -private: - rclcpp::Logger logger_; - - double lifting_velocity_; - - InputInterface lifting_command_; - OutputInterface lifting_left_vel_; - OutputInterface lifting_right_vel_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartFilling, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp deleted file mode 100644 index 8a0d9ac8..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_filling_test.cpp +++ /dev/null @@ -1,206 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -DartFillingTest — 新方案综合验证控制器 -键位： -双下 / UNKNOWN：全部停止 - -左拨杆中：调整角度模式 - 左摇杆 Y → yaw 电机力矩 - 右摇杆 X → pitch 电机力矩 - 右摇杆 Y → 传送带电机力矩（左右同步） - 右拨杆下拨再回中：降下抬升平台 - 右拨杆上拨再回中：抬起抬升平台 - -左拨杆上：发射/弹仓控制模式 - 右摇杆 X → 螺旋推力电机力矩 - 右拨杆下拨再回中：锁定扳机 + 限位舵机锁定 - 右拨杆上拨再回中：释放扳机 + 限位舵机释放 -*/ - -namespace rmcs_core::controller::dart { - -class DartFillingTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartFillingTest() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , timer_interval_ms_(10) - , logger_(get_logger()) { - - // Trigger servo: PWM value (double 0.0–1.0) - trigger_free_value_ = get_parameter("trigger_free_value").as_double(); - trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); - - max_torque_ = get_parameter("max_torque").as_double(); - max_belt_torque_ = get_parameter("max_belt_torque").as_double(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - - register_input("/dart/lifting_left/angle", lifting_left_angle_); - register_input("/dart/lifting_right/angle", lifting_right_angle_); - - // Trigger servo: PWM double value - register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); - - // Motor torque outputs (direct control, no PID needed in test) - register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); - register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); - register_output("/dart/force_screw_motor/control_torque", force_screw_torque_, 0.0); - register_output("/dart/drive_belt/left/control_torque", belt_left_torque_, 0.0); - register_output("/dart/drive_belt/right/control_torque", belt_right_torque_, 0.0); - - register_output("/dart/lifting_left/control_velocity", lifting_left_angle_shift_); - register_output("/dart/lifting_right/control_velocity", lifting_right_angle_shift_); - register_output("/dart/filling/stage", filling_stage_); - register_output("/force/sensor/average", average_force_); - register_output("/dart/limiting_servo/control_angle", limiting_control_angle_); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(timer_interval_ms_), - [this] { timer_callback(); }); - } - - void update() override { - using namespace rmcs_msgs; - - auto sw_left = *switch_left_; - auto sw_right = *switch_right_; - - const bool all_down = (sw_left == Switch::DOWN || sw_left == Switch::UNKNOWN) - && (sw_right == Switch::DOWN || sw_right == Switch::UNKNOWN); - - if (all_down) { - disable_all(); - } else if (sw_left == Switch::MIDDLE) { - // Angle / belt control mode - *yaw_torque_ = std::clamp(joystick_left_->y() * max_torque_, - -max_torque_, max_torque_); - *pitch_torque_ = std::clamp(joystick_right_->x() * max_torque_, - -max_torque_, max_torque_); - - double lifting_cmd = joystick_right_->y() * max_belt_torque_; - *lifting_left_angle_shift_ = lifting_cmd; - *lifting_right_angle_shift_ = -lifting_cmd; - - *force_screw_torque_ = 0.0; - - } else if (sw_left == Switch::UP) { - // Force-screw / trigger control mode - *force_screw_torque_ = std::clamp(joystick_right_->x() * max_torque_, - -max_torque_, max_torque_); - *yaw_torque_ = 0.0; - *pitch_torque_ = 0.0; - *belt_left_torque_ = 0.0; - *belt_right_torque_ = 0.0; - - // Right switch transitions: trigger / limiting servo - if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::DOWN) { - *trigger_value_ = trigger_lock_value_; - } else if (last_switch_right_ == Switch::MIDDLE && sw_right == Switch::UP) { - *trigger_value_ = trigger_free_value_; - } - } - - *average_force_ = (*force_sensor_ch1_data_ + *force_sensor_ch2_data_) * 0.5; - log_count_++; - if (log_count_ >= 1000) { - log_count_ = 0; - RCLCPP_INFO(logger_, "[ForceSensor] ch1: %d ch2: %d avg: %.1f", - *force_sensor_ch1_data_, *force_sensor_ch2_data_, *average_force_); - RCLCPP_INFO(logger_, "[Lifting] left: %.4f rad right: %.4f rad", - *lifting_left_angle_, *lifting_right_angle_); - } - - *limiting_control_angle_ = 0; - last_switch_left_ = sw_left; - last_switch_right_ = sw_right; - } - -private: - void disable_all() { - *trigger_value_ = trigger_lock_value_; - *yaw_torque_ = 0.0; - *pitch_torque_ = 0.0; - *force_screw_torque_ = 0.0; - *belt_left_torque_ = 0.0; - *belt_right_torque_ = 0.0; - } - - rclcpp::TimerBase::SharedPtr timer_; - int timer_interval_ms_; - std::function delayed_action_; - bool is_delaying_ = false; - int delay_remaining_ms_ = 0; - - void timer_callback() { - if (is_delaying_ && delay_remaining_ms_ > 0) { - delay_remaining_ms_ -= timer_interval_ms_; - if (delay_remaining_ms_ <= 0) { - is_delaying_ = false; - if (delayed_action_) - delayed_action_(); - } - } - } - - int log_count_ = 0; - - rclcpp::Logger logger_; - - // Remote inputs - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - - // Servo outputs - double trigger_free_value_; - double trigger_lock_value_; - OutputInterface trigger_value_; - - // Motor torque outputs (direct) - double max_torque_; - double max_belt_torque_; - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; - OutputInterface yaw_torque_; - OutputInterface pitch_torque_; - OutputInterface force_screw_torque_; - OutputInterface belt_left_torque_; - OutputInterface belt_right_torque_; - OutputInterface lifting_left_angle_shift_; - OutputInterface lifting_right_angle_shift_; - - // LK lifting motor angle feedback (radians, multi-turn) - InputInterface lifting_left_angle_; - InputInterface lifting_right_angle_; - OutputInterface limiting_control_angle_; - OutputInterface average_force_; - - // Stage output (used by broadcaster) - OutputInterface filling_stage_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFillingTest, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp deleted file mode 100644 index 098d1542..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/debug_info.cpp +++ /dev/null @@ -1,272 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -namespace rmcs_core::controller::dart { -class Test - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Test() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - register_input("/dart/pitch_motor/control_torque", pitch_control_torque_); - register_input("/dart/yaw_motor/control_torque", yaw_control_torque_); - register_input("/dart/pitch_motor/velocity", pitch_speed_); - register_input("/dart/yaw_motor/velocity", yaw_speed_); - register_input("/imu/catapult_roll_angle", final_roll_); - register_input("/imu/catapult_pitch_angle", final_pitch_); - register_input("/imu/catapult_yaw_angle", final_yaw_); - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_data_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_data_); - register_input("/dart/lifting_left/current_angle", lifting_angle_left_); - register_input("/dart/lifting_right/current_angle", lifting_angle_right_); - register_input("/dart/lifting_left/control_angle", lifting_control_angle_left_); - register_input("/dart/lifting_right/control_angle", lifting_control_angle_right_); - - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - - declare_parameter("fit_window_size", 50); - declare_parameter("fit_threshold", 1.0); - declare_parameter("stable_min_points", 20); - declare_parameter("stable_region_timeout", 500); - - fit_window_size_ = static_cast(get_parameter("fit_window_size").as_int()); - fit_threshold_ = get_parameter("fit_threshold").as_double(); - stable_min_points_ = static_cast(get_parameter("stable_min_points").as_int()); - stable_region_timeout_ = static_cast(get_parameter("stable_region_timeout").as_int()); - - total_query_pairs_ = 0; - sync_query_pairs_ = 0; - left_has_pending_ = false; - right_has_pending_ = false; - left_pending_delta_ = 0; - right_pending_delta_ = 0; - } - - void update() override { - // if (count_n % 100 == 0) { - // RCLCPP_INFO(logger_, "left_force: %d, right_force: %d", - // *force_sensor_ch1_data_, *force_sensor_ch2_data_); - // } - // count_n++; - if (*lifting_angle_left_ != last_left_angle_for_sync_) { - int16_t delta = calculateAngleDelta(last_left_angle_for_sync_, *lifting_angle_left_); - last_left_angle_for_sync_ = *lifting_angle_left_; - left_has_pending_ = true; - left_pending_delta_ = delta; - } - - if (*lifting_angle_right_ != last_right_angle_for_sync_) { - int16_t delta = calculateAngleDelta(last_right_angle_for_sync_, *lifting_angle_right_); - last_right_angle_for_sync_ = *lifting_angle_right_; - right_has_pending_ = true; - right_pending_delta_ = delta; - } - - if (left_has_pending_ && right_has_pending_) { - total_query_pairs_++; - int16_t delta_diff = std::abs(left_pending_delta_ - right_pending_delta_); - if (delta_diff <= 3) { - sync_query_pairs_++; - } else { - RCLCPP_WARN(logger_, "Lift sync mismatch: left delta=%d, right delta=%d, diff=%d", - left_pending_delta_, right_pending_delta_, delta_diff); - } - left_has_pending_ = false; - right_has_pending_ = false; - } - - recordAngleToWindow(); - - checkStableRegion(); - - if (is_in_stable_region_) { - calculateVibrationInStableRegion(); - } - - if (count_n % 1000 == 0) { - syn = (count_n > 1) ? (double)sync_coefficient_count_ / (count_n - 1) : 0.0; - double sync_ratio = (total_query_pairs_ > 0) ? (double)sync_query_pairs_ / total_query_pairs_ : 0.0; - - if (total_stable_points_ > 0) { - vib = (double)stable_at_control_angle_count_ / total_stable_points_; - RCLCPP_INFO(logger_, - "left_angle=%d, right_angle=%d | " - "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=%.4f", - *lifting_angle_left_, *lifting_angle_right_, - syn, sync_ratio, sync_query_pairs_, total_query_pairs_, vib); - } else { - RCLCPP_INFO(logger_, - "left_angle=%d, right_angle=%d | " - "cumulative_sync=%.4f, per_change_sync=%.4f (pairs=%ld/%ld), vib=N/A", - *lifting_angle_left_, *lifting_angle_right_, - syn, sync_ratio, sync_query_pairs_, total_query_pairs_); - } - } - - count_n++; - } - -private: - static int16_t calculateAngleDelta(uint16_t last_angle, uint16_t current_angle) { - int16_t delta = static_cast(current_angle - last_angle); - - if (delta > 32767) { - delta -= static_cast(65536); - } else if (delta < -32768) { - delta += static_cast(65536); - } - - return delta; - } - - void recordAngleToWindow() { - angle_window_.push_back(*lifting_angle_left_); - if (angle_window_.size() > fit_window_size_) { - angle_window_.pop_front(); - } - } - - void checkStableRegion() { - if (angle_window_.size() < stable_min_points_) { - stable_region_timeout_counter_ = 0; - return; - } - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - size_t n = angle_window_.size(); - - for (size_t i = 0; i < n; ++i) { - double x = static_cast(i); - double y = static_cast(angle_window_[i]); - sum_x += x; - sum_y += y; - sum_xy += x * y; - sum_xx += x * x; - } - - double b = (n * sum_xy - sum_x * sum_y) / (n * sum_xx - sum_x * sum_x); - double a = (sum_y - b * sum_x) / n; - - double residual_sum = 0.0; - for (size_t i = 0; i < n; ++i) { - double x = static_cast(i); - double y = static_cast(angle_window_[i]); - double y_fit = a + b * x; - residual_sum += (y - y_fit) * (y - y_fit); - } - - double rms_error = sqrt(residual_sum / n); - - bool is_stable = (std::abs(b) < 0.05 && rms_error < fit_threshold_); - - if (is_stable) { - if (!is_in_stable_region_) { - is_in_stable_region_ = true; - current_stable_start_point_ = angle_window_[angle_window_.size() - 1]; - stable_region_timeout_counter_ = 0; - - // RCLCPP_DEBUG(logger_, "Enter stable region: slope=%.4f, rms=%.4f", b, rms_error); - } - } else { - if (is_in_stable_region_) { - stable_region_timeout_counter_++; - if (stable_region_timeout_counter_ > stable_region_timeout_) { - is_in_stable_region_ = false; - stable_region_timeout_counter_ = 0; - - if (current_stable_total_points_ > 0) { - double stable_ratio = static_cast(current_stable_at_control_angle_) / - current_stable_total_points_; - // RCLCPP_DEBUG(logger_, "Exit stable region: total=%d, at control=%d, ratio=%.4f", - // current_stable_total_points_, current_stable_at_control_angle_, stable_ratio); - } - - current_stable_total_points_ = 0; - current_stable_at_control_angle_ = 0; - } - } - } - } - - void calculateVibrationInStableRegion() { - current_stable_total_points_++; - if (*lifting_angle_left_ == *lifting_control_angle_left_ && - *lifting_angle_right_ == *lifting_control_angle_right_) { - current_stable_at_control_angle_++; - } - - total_stable_points_++; - if (*lifting_angle_left_ == *lifting_control_angle_left_ && - *lifting_angle_right_ == *lifting_control_angle_right_) { - stable_at_control_angle_count_++; - } - } - -private: - int count_n = 1; - int sync_coefficient_count_ = 0; - rclcpp::Logger logger_; - double syn = 0.0; - double vib = 0.0; - - uint16_t last_left_angle_for_sync_ = 0; - uint16_t last_right_angle_for_sync_ = 0; - bool left_has_pending_; - bool right_has_pending_; - int16_t left_pending_delta_; - int16_t right_pending_delta_; - int64_t total_query_pairs_; - int64_t sync_query_pairs_; - - std::deque angle_window_; - - bool is_in_stable_region_ = false; - int stable_region_timeout_counter_ = 0; - uint16_t current_stable_start_point_ = 0; - - int current_stable_total_points_ = 0; - int current_stable_at_control_angle_ = 0; - - int total_stable_points_ = 0; - int stable_at_control_angle_count_ = 0; - - int fit_window_size_ = 50; - double fit_threshold_ = 1.0; - int stable_min_points_ = 20; - int stable_region_timeout_ = 500; - - InputInterface pitch_control_torque_; - InputInterface yaw_control_torque_; - InputInterface pitch_speed_; - InputInterface yaw_speed_; - InputInterface final_pitch_; - InputInterface final_roll_; - InputInterface final_yaw_; - InputInterface force_sensor_ch1_data_; - InputInterface force_sensor_ch2_data_; - InputInterface lifting_angle_left_; - InputInterface lifting_angle_right_; - InputInterface lifting_control_angle_left_; - InputInterface lifting_control_angle_right_; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_middle_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_middle_angle_right_; - uint16_t lifting_down_angle_right_; -}; -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::Test, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp index f3fa9d03..7309e59f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -12,13 +12,22 @@ namespace rmcs_core::controller::dart { -// Subscribes to DartManager belt/trigger commands and outputs motor torques + trigger servo value. -// Belt velocity control uses MatrixPidCalculator<2> with sync compensation. -class DartLaunchSetting +// DartLaunchSettingV2 — DartLaunchSetting 的升级版，新增 LK 升降电机速度控制。 +// +// 升降控制逻辑（仅做命令→速度映射，无堵转检测）： +// UP → left = +lifting_velocity, right = -lifting_velocity +// DOWN → left = -lifting_velocity, right = +lifting_velocity +// WAIT → left = right = 0（电机以小电流维持当前位置） +// +// 堵转检测由 LiftingLkAction 内部完成（直接读速度反馈），不在此处处理。 +// +// yaml 新增参数： +// lifting_velocity (double, rad/s) — 升降速度，正值=左+右-（平台上升方向），需实测标定 +class DartLaunchSettingV2 : public rmcs_executor::Component , public rclcpp::Node { public: - DartLaunchSetting() + DartLaunchSettingV2() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) @@ -32,44 +41,89 @@ class DartLaunchSetting max_control_torque_ = get_parameter("max_control_torque").as_double(); trigger_free_value_ = get_parameter("trigger_free_value").as_double(); trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); + lifting_velocity_ = get_parameter("lifting_velocity").as_double(); + belt_hold_torque_ = get_parameter("belt_hold_torque").as_double(); register_input("/dart/manager/belt/command", belt_command_); register_input("/dart/manager/trigger/lock_enable", trigger_lock_enable_); register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); - - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); + register_input("/dart/manager/lifting/command", lifting_command_); + // 第一发下降速度缩放因子（shot_count==1 时 0.8，其余 1.0） + register_input("/dart/manager/belt/down_scale", belt_down_scale_); + // 归零模式：true 时将传送带扭矩上限限制到 10%，防止顶住机械限位过热 + register_input("/dart/manager/belt/homing", belt_homing_mode_); register_output( "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); register_output( "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); + register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); + register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); } void update() override { double control_velocity = 0.0; switch (*belt_command_) { case rmcs_msgs::DartSliderStatus::DOWN: - control_velocity = +belt_velocity_; + control_velocity = +belt_velocity_ * (*belt_down_scale_); + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; break; case rmcs_msgs::DartSliderStatus::UP: control_velocity = -belt_velocity_; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; break; - case rmcs_msgs::DartSliderStatus::WAIT: default: control_velocity = 0.0; break; } - drive_belt_sync_control(control_velocity); + double torque_limit = *belt_homing_mode_ + ? max_control_torque_ * 0.1 + : max_control_torque_; + drive_belt_sync_control(control_velocity, torque_limit); *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); + + // 升降电机速度控制（UP/DOWN/WAIT → 速度映射） + switch (*lifting_command_) { + case rmcs_msgs::DartSliderStatus::UP: + *lifting_left_vel_ = +lifting_velocity_; + *lifting_right_vel_ = -lifting_velocity_; + break; + case rmcs_msgs::DartSliderStatus::DOWN: + *lifting_left_vel_ = -lifting_velocity_; + *lifting_right_vel_ = +lifting_velocity_; + break; + default: + *lifting_left_vel_ = 0.0; + *lifting_right_vel_ = 0.0; + break; + } } private: - void drive_belt_sync_control(double set_velocity) { + void drive_belt_sync_control(double set_velocity, double torque_limit) { + // WAIT 状态：清除 PID 积分，避免残余扭矩 + // 上端（prev=UP）：完全清零，避免顶着限位持续出力 + // 下端（prev=DOWN）：施加保持扭矩，防止重力/弹力导致滑台回弹 + if (set_velocity == 0.0) { + belt_pid_.reset(); + if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { + *left_belt_torque_ = +belt_hold_torque_; + *right_belt_torque_ = +belt_hold_torque_; + RCLCPP_INFO(logger_, "belt_combating"); + } else { + *left_belt_torque_ = 0.0; + *right_belt_torque_ = 0.0; + RCLCPP_INFO(logger_, "belt_init done"); + } + return; + } + Eigen::Vector2d setpoint_error{ set_velocity - *left_belt_velocity_, set_velocity - *right_belt_velocity_}; @@ -80,10 +134,8 @@ class DartLaunchSetting Eigen::Vector2d control_torques = belt_pid_.update(setpoint_error) - sync_coefficient_ * relative_velocity; - *left_belt_torque_ = - std::clamp(control_torques[0], -max_control_torque_, max_control_torque_); - *right_belt_torque_ = - std::clamp(control_torques[1], -max_control_torque_, max_control_torque_); + *left_belt_torque_ = std::clamp(control_torques[0], -torque_limit, torque_limit); + *right_belt_torque_ = std::clamp(control_torques[1], -torque_limit, torque_limit); } rclcpp::Logger logger_; @@ -95,21 +147,30 @@ class DartLaunchSetting double max_control_torque_; double trigger_free_value_; double trigger_lock_value_; + double lifting_velocity_; + double belt_hold_torque_{0.0}; + + rmcs_msgs::DartSliderStatus prev_belt_cmd_{rmcs_msgs::DartSliderStatus::WAIT}; InputInterface belt_command_; InputInterface trigger_lock_enable_; InputInterface left_belt_velocity_; InputInterface right_belt_velocity_; + InputInterface force_sensor_ch1_, force_sensor_ch2_; - InputInterface force_sensor_ch1_,force_sensor_ch2_; + InputInterface lifting_command_; + InputInterface belt_down_scale_; + InputInterface belt_homing_mode_; OutputInterface left_belt_torque_; OutputInterface right_belt_torque_; OutputInterface trigger_value_; + OutputInterface lifting_left_vel_; + OutputInterface lifting_right_vel_; }; } // namespace rmcs_core::controller::dart #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchSetting, rmcs_executor::Component) + rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp deleted file mode 100644 index 7309e59f..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting_v2.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "controller/pid/matrix_pid_calculator.hpp" - -namespace rmcs_core::controller::dart { - -// DartLaunchSettingV2 — DartLaunchSetting 的升级版，新增 LK 升降电机速度控制。 -// -// 升降控制逻辑（仅做命令→速度映射，无堵转检测）： -// UP → left = +lifting_velocity, right = -lifting_velocity -// DOWN → left = -lifting_velocity, right = +lifting_velocity -// WAIT → left = right = 0（电机以小电流维持当前位置） -// -// 堵转检测由 LiftingLkAction 内部完成（直接读速度反馈），不在此处处理。 -// -// yaml 新增参数： -// lifting_velocity (double, rad/s) — 升降速度，正值=左+右-（平台上升方向），需实测标定 -class DartLaunchSettingV2 - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchSettingV2() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) - , belt_pid_{ - get_parameter("b_kp").as_double(), - get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()} { - - belt_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - trigger_free_value_ = get_parameter("trigger_free_value").as_double(); - trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); - lifting_velocity_ = get_parameter("lifting_velocity").as_double(); - belt_hold_torque_ = get_parameter("belt_hold_torque").as_double(); - - register_input("/dart/manager/belt/command", belt_command_); - register_input("/dart/manager/trigger/lock_enable", trigger_lock_enable_); - register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); - register_input("/dart/manager/lifting/command", lifting_command_); - // 第一发下降速度缩放因子（shot_count==1 时 0.8，其余 1.0） - register_input("/dart/manager/belt/down_scale", belt_down_scale_); - // 归零模式：true 时将传送带扭矩上限限制到 10%，防止顶住机械限位过热 - register_input("/dart/manager/belt/homing", belt_homing_mode_); - - register_output( - "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); - register_output( - "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); - register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); - register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); - register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); - } - - void update() override { - double control_velocity = 0.0; - switch (*belt_command_) { - case rmcs_msgs::DartSliderStatus::DOWN: - control_velocity = +belt_velocity_ * (*belt_down_scale_); - prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; - break; - case rmcs_msgs::DartSliderStatus::UP: - control_velocity = -belt_velocity_; - prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; - break; - default: - control_velocity = 0.0; - break; - } - double torque_limit = *belt_homing_mode_ - ? max_control_torque_ * 0.1 - : max_control_torque_; - drive_belt_sync_control(control_velocity, torque_limit); - - *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; - // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); - - // 升降电机速度控制（UP/DOWN/WAIT → 速度映射） - switch (*lifting_command_) { - case rmcs_msgs::DartSliderStatus::UP: - *lifting_left_vel_ = +lifting_velocity_; - *lifting_right_vel_ = -lifting_velocity_; - break; - case rmcs_msgs::DartSliderStatus::DOWN: - *lifting_left_vel_ = -lifting_velocity_; - *lifting_right_vel_ = +lifting_velocity_; - break; - default: - *lifting_left_vel_ = 0.0; - *lifting_right_vel_ = 0.0; - break; - } - } - -private: - void drive_belt_sync_control(double set_velocity, double torque_limit) { - // WAIT 状态：清除 PID 积分，避免残余扭矩 - // 上端（prev=UP）：完全清零，避免顶着限位持续出力 - // 下端（prev=DOWN）：施加保持扭矩，防止重力/弹力导致滑台回弹 - if (set_velocity == 0.0) { - belt_pid_.reset(); - if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { - *left_belt_torque_ = +belt_hold_torque_; - *right_belt_torque_ = +belt_hold_torque_; - RCLCPP_INFO(logger_, "belt_combating"); - } else { - *left_belt_torque_ = 0.0; - *right_belt_torque_ = 0.0; - RCLCPP_INFO(logger_, "belt_init done"); - } - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_belt_velocity_, - set_velocity - *right_belt_velocity_}; - Eigen::Vector2d relative_velocity{ - *left_belt_velocity_ - *right_belt_velocity_, - *right_belt_velocity_ - *left_belt_velocity_}; - - Eigen::Vector2d control_torques = - belt_pid_.update(setpoint_error) - sync_coefficient_ * relative_velocity; - - *left_belt_torque_ = std::clamp(control_torques[0], -torque_limit, torque_limit); - *right_belt_torque_ = std::clamp(control_torques[1], -torque_limit, torque_limit); - } - - rclcpp::Logger logger_; - - pid::MatrixPidCalculator<2> belt_pid_; - - double belt_velocity_; - double sync_coefficient_; - double max_control_torque_; - double trigger_free_value_; - double trigger_lock_value_; - double lifting_velocity_; - double belt_hold_torque_{0.0}; - - rmcs_msgs::DartSliderStatus prev_belt_cmd_{rmcs_msgs::DartSliderStatus::WAIT}; - - InputInterface belt_command_; - InputInterface trigger_lock_enable_; - InputInterface left_belt_velocity_; - InputInterface right_belt_velocity_; - InputInterface force_sensor_ch1_, force_sensor_ch2_; - - InputInterface lifting_command_; - InputInterface belt_down_scale_; - InputInterface belt_homing_mode_; - - OutputInterface left_belt_torque_; - OutputInterface right_belt_torque_; - OutputInterface trigger_value_; - OutputInterface lifting_left_vel_; - OutputInterface lifting_right_vel_; -}; - -} // namespace rmcs_core::controller::dart - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp deleted file mode 100644 index 5b6ffea5..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/pwm_test.cpp +++ /dev/null @@ -1,28 +0,0 @@ - -#include -#include - -namespace rmcs_core::controller::test { -class PWMTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - PWMTest() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { - set_value_ = get_parameter("set_value").as_double(); - register_output("/dart/trigger_servo/value", value_); - } - - void update() override { *value_ = set_value_; } - -private: - double set_value_ = 0.0; - OutputInterface value_; -}; -} // namespace rmcs_core::controller::test - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::test::PWMTest, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp deleted file mode 100644 index e89f6707..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/servo_test.cpp +++ /dev/null @@ -1,97 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -namespace rmcs_core::hardware { -class ServoTest - : public rmcs_executor::Component - , public rclcpp::Node { -public: - ServoTest() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - trigger_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - trigger_free_angle_ = get_parameter("trigger_free_angle").as_int(); - lifting_up_angle_left_ = get_parameter("lifting_up_angle_left").as_int(); - lifting_middle_angle_left_ = get_parameter("lifting_middle_angle_left").as_int(); - lifting_down_angle_left_ = get_parameter("lifting_down_angle_left").as_int(); - lifting_up_angle_right_ = get_parameter("lifting_up_angle_right").as_int(); - lifting_middle_angle_right_ = get_parameter("lifting_middle_angle_right").as_int(); - lifting_down_angle_right_ = get_parameter("lifting_down_angle_right").as_int(); - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - register_output("/dart/lifting_left/control_angle", lifting_left_control_angle); - register_output("/dart/lifting_right/control_angle", lifting_right_control_angle); - - } - - void update() override { - using namespace rmcs_msgs; - - *trigger_control_angle = trigger_lock_angle_; - - if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { - *lifting_left_control_angle = lifting_down_angle_left_; - *lifting_right_control_angle = lifting_down_angle_right_; - } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { - *lifting_left_control_angle = lifting_up_angle_left_; - *lifting_right_control_angle = lifting_up_angle_right_; - } - - // if (*switch_left_ == Switch::UP && *switch_right_ == Switch::DOWN) { - // *trigger_control_angle = trigger_lock_angle_; - // } else if (*switch_left_ == Switch::UP && *switch_right_ == Switch::UP) { - // *trigger_control_angle = trigger_free_angle_; - // } - } - -private: - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - - uint16_t trigger_lock_angle_; - uint16_t trigger_free_angle_; - OutputInterface trigger_control_angle; - OutputInterface lifting_left_control_angle; - OutputInterface lifting_right_control_angle; - - uint16_t lifting_up_angle_left_; - uint16_t lifting_middle_angle_left_; - uint16_t lifting_down_angle_left_; - uint16_t lifting_up_angle_right_; - uint16_t lifting_middle_angle_right_; - uint16_t lifting_down_angle_right_; - - -}; -} // namespace rmcs_core::hardware - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::ServoTest, rmcs_executor::Component) - -// FF FF 01 03 03 16 E2 -// FF FF 01 04 02 16 02 E0 - -// FF FF 01 03 03 18 E0 -// FF FF 01 04 02 18 02 DE - -// FF FF 01 05 03 1C 00 00 DB -// FF FF 01 05 03 1C 00 01 DA - -// FF FF 01 07 03 41 00 14 00 00 9F -// FF FF 01 07 03 41 00 32 00 00 81 - -// FF FF 01 05 03 41 00 14 A1 -// FF FF 01 04 02 1D 01 DA \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp deleted file mode 100644 index 0a4913a3..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/dart_setttings.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include -#include -#include -#include -#include -#include - -/* -angle controls & launch parameter settings -Version: 1.0.0 (manual control) -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -class DartSettings - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartSettings() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) { - log_enable_ = get_parameter("log_enable").as_bool(); - force_screw_max_velocity_ = get_parameter("screw_max_velocity").as_double(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/force_sensor/weight/channel1", force_sensor_ch1_weight_); - register_input("/force_sensor/weight/channel2", force_sensor_ch2_weight_); - - register_output("/dart/force_screw_motor/control_velocity", force_screw_control_velocity_); - register_output("/dart/yaw_motor/control_velocity", yaw_control_velocity_); - register_output("/dart/pitch_motor/control_velocity", pitch_control_velocity_); - } - - void update() override { - if (*switch_left_ == rmcs_msgs::Switch::UP) { - if (*switch_right_ == rmcs_msgs::Switch::DOWN) { - *force_screw_control_velocity_ = joystick_right_->x() * force_screw_max_velocity_; - } - if (*switch_right_ == rmcs_msgs::Switch::MIDDLE) { - *yaw_control_velocity_ = joystick_right_->y() * angle_control_max_velocity_; - *pitch_control_velocity_ = joystick_left_->x() * angle_control_max_velocity_; - } - } else { - reset(); - } - - if (log_enable_) { - force_sensor_measurement_log(); - } - } - -private: - void reset() { - *force_screw_control_velocity_ = 0; - *yaw_control_velocity_ = nan_; - *pitch_control_velocity_ = nan_; - } - - void force_sensor_measurement_log() { - if (log_count_++ > 100) { - log_count_ = 0; - RCLCPP_INFO( - logger_, "ch1: %d | ch2: %d", *force_sensor_ch1_weight_, *force_sensor_ch2_weight_); - } - } - - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - - OutputInterface yaw_control_velocity_; - OutputInterface pitch_control_velocity_; - - InputInterface force_sensor_ch1_weight_, force_sensor_ch2_weight_; - OutputInterface force_screw_control_velocity_; - double force_screw_max_velocity_; - double angle_control_max_velocity_; - - bool log_enable_ = false; - int log_count_ = 0; -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettings, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp deleted file mode 100644 index 3a068201..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/dartlauncher/launch_controller.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "controller/pid/matrix_pid_calculator.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* -键位： -双下：全部停止 -双中：初始状态 - 此时{ - 右拨杆下拨再回中：切换上膛和退膛 - 处于上膛状态时右拨杆打到上：发射 - } -左拨杆上：设置模式 - 此时{ - 右拨杆在中：调整角度，左右摇杆分别控制yaw和pitch以防误触 - 右拨杆在下：调整拉力，在yaml中设置力闭环模式或者手动控制模式 - } -*/ - -namespace rmcs_core::controller::dart { -class DartLaunchController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DartLaunchController() - : Node{ - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) - , drive_belt_pid_calculator_( - get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), - get_parameter("b_kd").as_double()) { - - dirve_belt_working_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - - launch_trigger_angle_ = get_parameter("trigger_free_angle").as_int(); - launch_lock_angle_ = get_parameter("trigger_lock_angle").as_int(); - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - - register_input("/dart/drive_belt/left/velocity", left_drive_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_drive_belt_velocity_); - - register_output("/dart/drive_belt/left/control_torque", left_drive_belt_control_torque_, 0); - register_output( - "/dart/drive_belt/right/control_torque", right_drive_belt_control_torque_, 0); - - register_output("/dart/trigger_servo/control_angle", trigger_control_angle); - } - - void update() override { - using namespace rmcs_msgs; - - if ((*switch_left_ == Switch::DOWN || *switch_left_ == Switch::UNKNOWN) - && (*switch_right_ == Switch::DOWN || *switch_right_ == Switch::UNKNOWN)) { - *launch_stage_ = DartLaunchStages::DISABLE; - reset_all_controls(); - - } else if (*switch_left_ == Switch::MIDDLE) { - - if (last_launch_stage_ == DartLaunchStages::DISABLE) { - *launch_stage_ = DartLaunchStages::RESETTING; - } - - if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::DOWN) { - if (last_launch_stage_ == DartLaunchStages::INIT) { - *launch_stage_ = DartLaunchStages::LOADING; - } else if (last_launch_stage_ == DartLaunchStages::READY) { - *launch_stage_ = DartLaunchStages::CANCEL; - } - } else if (last_switch_right_ == Switch::MIDDLE && *switch_right_ == Switch::UP) { - if (last_launch_stage_ == DartLaunchStages::READY) { - *launch_stage_ = DartLaunchStages::INIT; - trigger_lock_flag_ = false; - } else { - RCLCPP_INFO(logger_, "Dart has't been loaded !"); - } - } - - if (blocking_detection()) { - if (last_launch_stage_ == DartLaunchStages::LOADING) { - *launch_stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = true; - dirve_belt_block_count_ = 0; - - } else if (last_launch_stage_ == DartLaunchStages::CANCEL) { - *launch_stage_ = DartLaunchStages::RESETTING; - trigger_lock_flag_ = false; - dirve_belt_block_count_ = 0; - - } else if (last_launch_stage_ == DartLaunchStages::RESETTING) { - *launch_stage_ = - trigger_lock_flag_ ? DartLaunchStages::READY : DartLaunchStages::INIT; - dirve_belt_block_count_ = 0; - } - } - double control_velocity = 0; - double control_torque_limit = max_control_torque_; - - if (*launch_stage_ == rmcs_msgs::DartLaunchStages::RESETTING) { - control_velocity = -dirve_belt_working_velocity_; - control_torque_limit = max_control_torque_ * 0.1; - } else if ( - *launch_stage_ == rmcs_msgs::DartLaunchStages::LOADING - || *launch_stage_ == rmcs_msgs::DartLaunchStages::CANCEL) { - control_velocity = dirve_belt_working_velocity_; - control_torque_limit = max_control_torque_; - } else { - control_velocity = 0; - } - drive_belt_sync_control(control_velocity, control_torque_limit); - // RCLCPP_INFO(logger_, "%lf", control_velocity); - } - - *trigger_control_angle = trigger_lock_flag_ ? launch_lock_angle_ : launch_trigger_angle_; - - last_switch_left_ = *switch_left_; - last_switch_right_ = *switch_right_; - last_launch_stage_ = *launch_stage_; - } - -private: - void reset_all_controls() { - *launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - } - - void drive_belt_sync_control(double set_velocity, double control_torque_limit) { - if (set_velocity == 0) { - *left_drive_belt_control_torque_ = 0; - *right_drive_belt_control_torque_ = 0; - return; - } - - Eigen::Vector2d setpoint_error{ - set_velocity - *left_drive_belt_velocity_, set_velocity - *right_drive_belt_velocity_}; - - Eigen::Vector2d relative_velocity{ - *left_drive_belt_velocity_ - *right_drive_belt_velocity_, - *right_drive_belt_velocity_ - *left_drive_belt_velocity_}; - - Eigen::Vector2d control_torques = setpoint_error - sync_coefficient_ * relative_velocity; - - *left_drive_belt_control_torque_ = - std::clamp(control_torques[0], -control_torque_limit, control_torque_limit); - *right_drive_belt_control_torque_ = - std::clamp(control_torques[1], -control_torque_limit, control_torque_limit); - } - - bool blocking_detection() { - if ((abs(*left_drive_belt_velocity_) < 0.5 && abs(*left_drive_belt_control_torque_) > 0.5) - || (abs(*right_drive_belt_velocity_) < 0.5 - && abs(*right_drive_belt_control_torque_) > 0.5)) { - dirve_belt_block_count_++; - } - - return dirve_belt_block_count_ > 1000 ? true : false; - } - - int dirve_belt_block_count_ = 0; - double dirve_belt_working_velocity_; - - rclcpp::Logger logger_; - - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - rmcs_msgs::Switch last_switch_right_; - rmcs_msgs::Switch last_switch_left_; - - OutputInterface left_drive_belt_control_torque_; - OutputInterface right_drive_belt_control_torque_; - InputInterface left_drive_belt_velocity_; - InputInterface right_drive_belt_velocity_; - - double max_control_torque_; - - OutputInterface launch_stage_; - rmcs_msgs::DartLaunchStages last_launch_stage_ = rmcs_msgs::DartLaunchStages::DISABLE; - - uint16_t launch_lock_angle_; - uint16_t launch_trigger_angle_; - bool trigger_lock_flag_ = false; - OutputInterface trigger_control_angle; - - pid::MatrixPidCalculator<2> drive_belt_pid_calculator_; - double sync_coefficient_; -}; - -} // namespace rmcs_core::controller::dart - -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartLaunchController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp deleted file mode 100644 index d8c2d5e8..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ /dev/null @@ -1,567 +0,0 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" -// #include "hardware/device/trigger_servo.hpp" - -// #include "filter/low_pass_filter.hpp" -// #include "hardware/device/bmi088.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/force_sensor_runtime.hpp" -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include - -// #include - -// // #include -// // #include -// // #include -// // #include - -// namespace rmcs_core::hardware { - -// class CatapultDart -// : public rmcs_executor::Component -// , public rclcpp::Node -// , public librmcs::agent::CBoard { -// public: -// CatapultDart() -// : Node{get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , -// librmcs::agent::CBoard{static_cast(get_parameter("usb_pid").as_int())} , -// logger_(get_logger()) , pitch_angle_filter_(20.0, 1000.0) , pitch_velocity_filter_(20.0, -// 1000.0) , yaw_velocity_filter_(20.0, 1000.0) , yaw_filter_(5.0, 1000.0) , -// pitch_filter_(10.0, 1000.0) , roll_filter_(10.0, 1000.0) , dart_command_( -// create_partner_component(get_component_name() + "_command", *this)) -// , dr16_(*this) -// , imu_(1000, 0.2, 0.0) -// , pitch_motor_( -// *this, *dart_command_, "/dart/pitch_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , yaw_motor_( -// *this, *dart_command_, "/dart/yaw_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , force_control_motor_( -// *this, *dart_command_, "/dart/force_control_motor", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)) -// , drive_belt_motor_( -// {*this, *dart_command_, "/dart/drive_belt/left", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio(19.)}, -// {*this, *dart_command_, "/dart/drive_belt/right", -// device::DjiMotor::Config{device::DjiMotor::Type::M3508} -// .set_reduction_ratio(19.) -// .set_reversed()}) -// , force_sensor_(*this) -// , trigger_servo_(*dart_command_, "/dart/trigger_servo", TRIGGER_SERVO_ID) -// , limiting_servo_(*dart_command_, "/dart/limiting_servo", LIMITING_SERVO_ID) -// , lifting_left_(*dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) -// , lifting_right_(*dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) -// , transmit_buffer_(*this, 32) -// , event_thread_([this]() { handle_events(); }) { -// register_output("/tf", tf_); -// register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); -// register_output("/imu/catapult_roll_angle", catapult_roll_angle_); -// register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); -// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); -// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - -// first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); -// final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - -// setup_imu_coordinate_mapping(); -// imu_sampler_initialize(); -// tf_broadcaster_ = std::make_unique(*this); -// start_time_ = std::chrono::steady_clock::now(); - -// trigger_calibrate_subscription_ = create_subscription( -// "/trigger/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// trigger_servo_, std::move(msg), trigger_servo_uart_data_ptr); -// }); -// limiting_calibrate_subscription_ = create_subscription( -// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) -// { -// trigger_servo_calibrate_subscription_callback( -// limiting_servo_, std::move(msg), limiting_servo_uart_data_ptr); -// }); -// lifting_left_calibrate_subscription_ = create_subscription( -// "/lifting_left/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// lifting_left_, std::move(msg), lifting_left_uart_data_ptr); -// }); -// lifting_right_calibrate_subscription_ = create_subscription( -// "/lifting_right/calibrate", rclcpp::QoS{0}, -// [this](std_msgs::msg::Int32::UniquePtr&& msg) { -// trigger_servo_calibrate_subscription_callback( -// lifting_right_, std::move(msg), lifting_right_uart_data_ptr); -// }); - -// 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; -// }; - -// last_read_left_time_ = this->now(); -// last_read_right_time_ = this->now(); -// } - -// ~CatapultDart() override { -// stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); -// pitch_motor_.update_status(); -// yaw_motor_.update_status(); -// drive_belt_motor_[0].update_status(); -// drive_belt_motor_[1].update_status(); -// force_control_motor_.update_status(); -// force_sensor_.update_status(); - -// imu_.update_status(); -// processImuData(); -// } - -// void command_update() { -// uint16_t can_commands[4]; - -// if (pub_time_count_++ > 100) { -// transmit_buffer_.add_can1_transmission( -// 0x301, std::bit_cast(force_sensor_.generate_command())); -// pub_time_count_ = 0; -// } - -// can_commands[0] = pitch_motor_.generate_command(); -// can_commands[1] = yaw_motor_.generate_command(); -// can_commands[2] = force_control_motor_.generate_command(); -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(can_commands)); - -// can_commands[0] = drive_belt_motor_[0].generate_command(); -// can_commands[1] = drive_belt_motor_[1].generate_command(); -// can_commands[2] = 0; -// can_commands[3] = 0; -// transmit_buffer_.add_can2_transmission(0x1FF, std::bit_cast(can_commands)); - -// if (!trigger_servo_.calibrate_mode()) { -// uint16_t current_target = trigger_servo_.get_target_angle(); -// if (current_target != last_trigger_angle_) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission( -// trigger_servo_uart_data_ptr, uart_data_length); -// last_trigger_angle_ = current_target; -// } -// } - -// if (!limiting_servo_.calibrate_mode()) { -// uint16_t current_target = limiting_servo_.get_target_angle(); -// if (current_target != last_limiting_angle_) { -// size_t uart_data_length; -// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_limiting_angle_ = current_target; -// } -// } - -// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { -// uint16_t current_target_left = lifting_left_.get_target_angle(); -// uint16_t current_target_right = lifting_right_.get_target_angle(); -// if (current_target_left != last_lifting_left_angle_) { -// size_t uart_data_length; -// uint16_t runtime_left = 0; -// uint16_t runtime_right = 0; -// std::unique_ptr command_buffer = -// device::TriggerServo::generate_sync_run_command( -// uart_data_length, LIFTING_LEFT_ID, LIFTING_RIGHT_ID, current_target_left, -// current_target_right, runtime_left, runtime_right); -// const auto lifting_table_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission( -// lifting_table_uart_data_ptr, uart_data_length); -// last_lifting_left_angle_ = current_target_left; -// } -// } - -// auto now = this->now(); - -// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { -// size_t uart_data_length; -// auto command_buffer = lifting_left_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_left_time_ = now; -// } -// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) -// { -// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { -// size_t uart_data_length; -// auto command_buffer = lifting_right_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, -// uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_right_time_ = now; -// } -// } - -// transmit_buffer_.trigger_transmission(); -// } - -// int pub_time_count_ = 0; - -// 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 == 0x302) { -// force_sensor_.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_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] -// return; - -// if (can_id == 0x201) { -// pitch_motor_.store_status(can_data); -// } else if (can_id == 0x202) { -// yaw_motor_.store_status(can_data); -// } else if (can_id == 0x203) { -// force_control_motor_.store_status(can_data); -// } else if (can_id == 0x205) { -// drive_belt_motor_[0].store_status(can_data); -// } else if (can_id == 0x206) { -// drive_belt_motor_[1].store_status(can_data); -// } -// } - -// void uart1_receive_callback(const std::byte* data, uint8_t length) override { -// referee_ring_buffer_receive_.emplace_back_multi( -// [&data](std::byte* storage) { *storage = *data++; }, length); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// std::string hex_string = bytes_to_hex_string(data, length); -// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, -// hex_string.c_str()); - -// if (length < 3) { -// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); -// return; -// } - -// uint8_t servo_id = static_cast(data[2]); -// std::pair result{false, 0}; - -// switch (servo_id) { -// case TRIGGER_SERVO_ID: -// result = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIMITING_SERVO_ID: -// result = limiting_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_LEFT_ID: -// result = lifting_left_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_left_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_RIGHT_ID: -// result = lifting_right_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_right_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); break; -// } -// } - -// 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 { -// imu_.store_accelerometer_status(x, y, z); -// } - -// void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { -// imu_.store_gyroscope_status(x, y, z); -// } - -// private: -// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; -// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; -// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; -// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; -// std::byte* trigger_servo_uart_data_ptr; -// std::byte* limiting_servo_uart_data_ptr; -// std::byte* lifting_left_uart_data_ptr; -// std::byte* lifting_right_uart_data_ptr; - -// void trigger_servo_calibrate_subscription_callback( -// device::TriggerServo& servo_, std_msgs::msg::Int32::UniquePtr msg, -// std::byte* servo_uart_data_ptr) { -// /* -// 标定命令格式： -// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" -// 替换data值就行 -// */ -// servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - -// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); -// RCLCPP_INFO( -// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, -// hex_string.c_str()); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } // DEBUG TOOL - -// void setup_imu_coordinate_mapping() { -// imu_.set_coordinate_mapping( -// [](double x, double y, double z) -> std::tuple { -// return {x, -y, -z}; -// }); -// } - -// void imu_sampler_initialize() { -// start_time_ = std::chrono::steady_clock::now(); -// yaw_drift_coefficient_ = 0.0; -// } - -// void processImuData() { -// auto current_time = std::chrono::steady_clock::now(); -// double elapsed_seconds = std::chrono::duration(current_time - -// start_time_).count(); - -// Eigen::Quaterniond imu_quaternion(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); -// Eigen::Matrix3d rotation_matrix = imu_quaternion.toRotationMatrix(); - -// double roll = std::atan2(rotation_matrix(2, 1), rotation_matrix(2, 2)); -// double pitch = std::asin(-rotation_matrix(2, 0)); -// double yaw = std::atan2(rotation_matrix(1, 0), rotation_matrix(0, 0)); - -// double transformed_roll = -roll_filter_.update(roll); -// double transformed_pitch = pitch_filter_.update(pitch); -// double transformed_yaw = -yaw_filter_.update(yaw); - -// if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { -// yaw_samples_.push_back(transformed_yaw); -// time_samples_.push_back(elapsed_seconds); -// double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; -// double n = static_cast(yaw_samples_.size()); - -// for (int i = 0; i < n; ++i) { -// sum_x += time_samples_[i]; -// sum_y += yaw_samples_[i]; -// sum_xy += time_samples_[i] * yaw_samples_[i]; -// sum_xx += time_samples_[i] * time_samples_[i]; -// } - -// double denominator = n * sum_xx - sum_x * sum_x; -// if (std::abs(denominator) > 1e-10) { -// yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; -// } -// } - -// transformed_yaw -= ((yaw_drift_coefficient_ + 0.000512) * elapsed_seconds); -// publishTfTransforms(transformed_roll, transformed_pitch, transformed_yaw); - -// *catapult_roll_angle_ = (transformed_roll / M_PI) * 180.0; -// *catapult_pitch_angle_ = (transformed_pitch / M_PI) * 180.0; -// *catapult_yaw_angle_ = (transformed_yaw / M_PI) * 180.0; -// } - -// void publishTfTransforms(double roll, double pitch, double yaw) { -// auto now = this->get_clock()->now(); - -// auto create_transform = [&](const std::string& parent, const std::string& child, -// const geometry_msgs::msg::Vector3& trans, -// const geometry_msgs::msg::Quaternion& rot) { -// geometry_msgs::msg::TransformStamped tf; -// tf.header.stamp = now; -// tf.header.frame_id = parent; -// tf.child_frame_id = child; -// tf.transform.translation = trans; -// tf.transform.rotation = rot; -// return tf; -// }; - -// auto create_translation = [](double x, double y, double z) { -// geometry_msgs::msg::Vector3 t; -// t.x = x; -// t.y = y; -// t.z = z; -// return t; -// }; - -// auto create_rotation = [](double x, double y, double z, double w) { -// geometry_msgs::msg::Quaternion r; -// r.x = x; -// r.y = y; -// r.z = z; -// r.w = w; -// return r; -// }; - -// geometry_msgs::msg::Vector3 zero_trans = create_translation(0, 0, 0); -// geometry_msgs::msg::Vector3 pitch_trans = create_translation(0, 0, 0.05); - -// Eigen::Quaterniond roll_quat(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); -// Eigen::Quaterniond pitch_quat(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); -// Eigen::Quaterniond yaw_quat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); -// Eigen::Quaterniond combined_quaternion = yaw_quat * pitch_quat * roll_quat; - -// tf_broadcaster_->sendTransform(create_transform( -// "base_link", "gimbal_center_link", zero_trans, create_rotation(0, 0, 0, 1))); -// tf_broadcaster_->sendTransform(create_transform( -// "gimbal_center_link", "yaw_link", zero_trans, -// create_rotation(yaw_quat.x(), yaw_quat.y(), yaw_quat.z(), yaw_quat.w()))); -// tf_broadcaster_->sendTransform(create_transform( -// "yaw_link", "pitch_link", pitch_trans, -// create_rotation(pitch_quat.x(), pitch_quat.y(), pitch_quat.z(), pitch_quat.w()))); -// tf_broadcaster_->sendTransform(create_transform( -// "pitch_link", "odom_imu", zero_trans, -// create_rotation( -// combined_quaternion.x(), combined_quaternion.y(), combined_quaternion.z(), -// combined_quaternion.w()))); -// tf_broadcaster_->sendTransform( -// create_transform("world", "base_link", zero_trans, create_rotation(0, 0, 0, 1))); -// } - -// rclcpp::Logger logger_; -// filter::LowPassFilter<1> pitch_angle_filter_; -// filter::LowPassFilter<1> pitch_velocity_filter_; -// filter::LowPassFilter<1> yaw_velocity_filter_; -// filter::LowPassFilter<1> yaw_filter_; -// filter::LowPassFilter<1> pitch_filter_; -// filter::LowPassFilter<1> roll_filter_; -// std::chrono::steady_clock::time_point start_time_; -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(CatapultDart& robot) -// : dart_(robot) {} -// void update() override { dart_.command_update(); } -// CatapultDart& dart_; -// }; - -// std::shared_ptr dart_command_; -// double first_sample_spot_; -// double final_sample_spot_; -// OutputInterface tf_; -// std::unique_ptr tf_broadcaster_; -// device::Dr16 dr16_; -// device::Bmi088 imu_; -// device::DjiMotor pitch_motor_; -// device::DjiMotor yaw_motor_; -// device::DjiMotor force_control_motor_; -// device::DjiMotor drive_belt_motor_[2]; -// device::ForceSensorRuntime force_sensor_; -// device::TriggerServo trigger_servo_; -// device::TriggerServo limiting_servo_; -// device::TriggerServo lifting_left_; -// device::TriggerServo lifting_right_; -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; -// librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; -// OutputInterface referee_serial_; -// librmcs::client::CBoard::TransmitBuffer transmit_buffer_; -// std::thread event_thread_; -// OutputInterface catapult_pitch_angle_; -// OutputInterface catapult_roll_angle_; -// OutputInterface catapult_yaw_angle_; -// OutputInterface lifting_current_angle_left_; -// OutputInterface lifting_current_angle_right_; -// double yaw_drift_coefficient_ = 0.0; -// std::vector yaw_samples_, time_samples_; -// uint16_t last_trigger_angle_ = 0xFFFF; -// uint16_t last_limiting_angle_ = 0xFFFF; -// uint16_t last_lifting_left_angle_ = 0xFFFF; -// rclcpp::Time last_read_left_time_; -// rclcpp::Time last_read_right_time_; -// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -// }; -// } // namespace rmcs_core::hardware - -// #include -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDart, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp deleted file mode 100644 index 1d78925f..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_filling_test.cpp +++ /dev/null @@ -1,160 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hardware/device/can_packet.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/trigger_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartFillingTest - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartFillingTest() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} - , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} - , limiting_servo_{*dart_command_, "/dart/limiting_servo", 0x04} - - , dr16_{*this} { - lifting_left_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - lifting_right_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - } - - CatapultDartFillingTest(const CatapultDartFillingTest&) = delete; - CatapultDartFillingTest& operator=(const CatapultDartFillingTest&) = delete; - CatapultDartFillingTest(CatapultDartFillingTest&&) = delete; - CatapultDartFillingTest& operator=(CatapultDartFillingTest&&) = delete; - - ~CatapultDartFillingTest() override = default; - - void update() override { - dr16_.update_status(); - lifting_left_motor_.update_status(); - lifting_right_motor_.update_status(); - } - - void command_update() { - auto board = start_transmit(); - - board.can1_transmit({ - .can_id = 0x141, - .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), - }); - - board.can1_transmit({ - .can_id = 0x145, - .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), - }); - - if (!limiting_servo_.calibrate_mode()) { - uint16_t current_target = limiting_servo_.get_target_angle(); - if (current_target != last_limiting_angle_) { - size_t uart_data_length; - auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); - board.uart2_transmit( - {.uart_data = std::span{command_buffer.get(), uart_data_length}}); - last_limiting_angle_ = current_target; - RCLCPP_INFO(logger_, "limiting_servo angle: 0x%04X", current_target); - } - } - } - -protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - std::lock_guard lock(referee_mutex_); - for (auto byte : data.uart_data) { - referee_ring_buffer_receive_.push_back(byte); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - - } - - void dbus_receive_callback(const librmcs::data::UartDataView& data) override { - dr16_.store_status(data.uart_data.data(), data.uart_data.size()); - } - -private: - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) - return "[]"; - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - for (size_t i = 0; i < size; ++i) - ss << std::setw(2) << static_cast(data[i]) << " "; - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') - result.pop_back(); - return result; - } - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartFillingTest& dart) - : dart_(dart) {} - void update() override { dart_.command_update();} - - private: - CatapultDartFillingTest& dart_; - }; - - std::shared_ptr dart_command_; - rclcpp::Logger logger_; - - uint16_t last_limiting_angle_{0xFFFF}; - - device::LkMotor lifting_left_motor_; - device::LkMotor lifting_right_motor_; - - device::TriggerServo limiting_servo_; - - device::Dr16 dr16_; - - std::mutex referee_mutex_; - std::deque referee_ring_buffer_receive_; - OutputInterface referee_serial_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartFillingTest, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp deleted file mode 100644 index 1419d0a8..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v3_full.cpp +++ /dev/null @@ -1,426 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "filter/low_pass_filter.hpp" -#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/force_sensor.hpp" -#include "hardware/device/pwm_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartV3Full - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartV3Full() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , pitch_angle_filter_(20.0, 1000.0) - , pitch_velocity_filter_(20.0, 1000.0) - , yaw_velocity_filter_(20.0, 1000.0) - , yaw_filter_(5.0, 1000.0) - , pitch_filter_(10.0, 1000.0) - , roll_filter_(10.0, 1000.0) - , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} - , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} - , force_screw_motor_{*this, *dart_command_, "/dart/force_screw_motor"} - , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} - , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} - , force_sensor_{*this} - , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} - - , dr16_{*this} - , imu_{1000, 0.2, 0.0} { - - pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - force_screw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_left_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_right_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - - first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); - final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - - setup_imu_coordinate_mapping(); - start_time_ = std::chrono::steady_clock::now(); - yaw_drift_coefficient_ = 0.0; - tf_broadcaster_ = std::make_unique(*this); - - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - - force_sensor_calibrate_subscription_ = create_subscription( - "/force_sensor/calibrate", rclcpp::QoS{0}, - [this](std_msgs::msg::Int32::UniquePtr&& msg) { - if (msg->data == 0) { - RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); - auto board = start_transmit(); - board.can1_transmit({ - .can_id = 0x201, - .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), - }); - } else { - RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); - } - } - ); - - register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { - std::lock_guard lock(referee_mutex_); - size_t count = 0; - while (count < size && !referee_ring_buffer_receive_.empty()) { - buffer[count++] = referee_ring_buffer_receive_.front(); - referee_ring_buffer_receive_.pop_front(); - } - return count; - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { - auto board = start_transmit(); - board.uart1_transmit({.uart_data = std::span{buffer, size}}); - return size; - }; - } - - CatapultDartV3Full(const CatapultDartV3Full&) = delete; - CatapultDartV3Full& operator=(const CatapultDartV3Full&) = delete; - CatapultDartV3Full(CatapultDartV3Full&&) = delete; - CatapultDartV3Full& operator=(CatapultDartV3Full&&) = delete; - - ~CatapultDartV3Full() override = default; - - void update() override { - dr16_.update_status(); - pitch_motor_.update_status(); - yaw_motor_.update_status(); - drive_belt_motor_left_.update_status(); - drive_belt_motor_right_.update_status(); - force_screw_motor_.update_status(); - force_sensor_.update_status(); - imu_.update_status(); - processImuData(); - } - - void command_update() { - auto board = start_transmit(); - - board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - - if (pub_time_count_++ > 100) { - board.can1_transmit({ - .can_id = 0x301, - .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), - }); - pub_time_count_ = 0; - } - - board.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - pitch_motor_.generate_command(), - yaw_motor_.generate_command(), - force_screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - - }); - - board.can2_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - drive_belt_motor_left_.generate_command(), - drive_belt_motor_right_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - }); - } - -protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - const auto can_id = data.can_id; - - if (can_id == 0x302) { - force_sensor_frame_count_++; - if (force_sensor_frame_count_ % 200 == 1) { - RCLCPP_INFO( - logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", - data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - force_sensor_.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) [[unlikely]] - return; - const auto can_id = data.can_id; - - // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID - if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 - && can_id != 0x205 && can_id != 0x206) { - can2_unknown_count_++; - if (can2_unknown_count_ % 50 == 1) { - RCLCPP_INFO( - logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", - can_id, data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - } - - if (can_id == 0x201) { - pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x203) { - force_screw_motor_.store_status(data.can_data); - } else if (can_id == 0x205) { - drive_belt_motor_left_.store_status(data.can_data); - } else if (can_id == 0x206) { - drive_belt_motor_right_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - std::lock_guard lock(referee_mutex_); - for (auto byte : data.uart_data) { - referee_ring_buffer_receive_.push_back(byte); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - } - - 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 { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - -private: - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) - return "[]"; - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - for (size_t i = 0; i < size; ++i) - ss << std::setw(2) << static_cast(data[i]) << " "; - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') - result.pop_back(); - return result; - } - - void setup_imu_coordinate_mapping() { - imu_.set_coordinate_mapping( - [](double x, double y, double z) -> std::tuple { - return {x, -y, -z}; - }); - } - - void processImuData() { - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = - std::chrono::duration(current_time - start_time_).count(); - - Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - - double roll = std::atan2(rot(2, 1), rot(2, 2)); - double pitch = std::asin(-rot(2, 0)); - double yaw = std::atan2(rot(1, 0), rot(0, 0)); - - double t_roll = -roll_filter_.update(roll); - double t_pitch = pitch_filter_.update(pitch); - double t_yaw = -yaw_filter_.update(yaw); - - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - yaw_samples_.push_back(t_yaw); - time_samples_.push_back(elapsed_seconds); - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = static_cast(yaw_samples_.size()); - for (size_t i = 0; i < yaw_samples_.size(); ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) - yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; - } - - t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; - publishTfTransforms(t_roll, t_pitch, t_yaw); - - *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; - *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; - } - - void publishTfTransforms(double roll, double pitch, double yaw) { - auto now = this->get_clock()->now(); - - auto make_tf = - [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; - - auto make_vec = [](double x, double y, double z) { - geometry_msgs::msg::Vector3 v; - v.x = x; v.y = y; v.z = z; - return v; - }; - - auto make_quat = [](double x, double y, double z, double w) { - geometry_msgs::msg::Quaternion q; - q.x = x; q.y = y; q.z = z; q.w = w; - return q; - }; - - Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); - Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - - auto zero = make_vec(0, 0, 0); - auto p_trans = make_vec(0, 0, 0.05); - auto id_rot = make_quat(0, 0, 0, 1); - - tf_broadcaster_->sendTransform( - make_tf("base_link", "gimbal_center_link", zero, id_rot)); - tf_broadcaster_->sendTransform(make_tf( - "gimbal_center_link", "yaw_link", zero, - make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "yaw_link", "pitch_link", p_trans, - make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "pitch_link", "odom_imu", zero, - make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); - tf_broadcaster_->sendTransform( - make_tf("world", "base_link", zero, id_rot)); - } - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartV3Full& dart) - : dart_(dart) {} - void update() override { dart_.command_update();} - - private: - CatapultDartV3Full& dart_; - }; - - std::shared_ptr dart_command_; - rclcpp::Logger logger_; - - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - filter::LowPassFilter<1> yaw_filter_; - filter::LowPassFilter<1> pitch_filter_; - filter::LowPassFilter<1> roll_filter_; - - std::chrono::steady_clock::time_point start_time_; - - device::DjiMotor pitch_motor_; - device::DjiMotor yaw_motor_; - device::DjiMotor force_screw_motor_; - device::DjiMotor drive_belt_motor_left_; - device::DjiMotor drive_belt_motor_right_; - - device::ForceSensor force_sensor_; - - device::PWMServo trigger_servo_; - - device::Dr16 dr16_; - device::Bmi088 imu_; - - std::unique_ptr tf_broadcaster_; - - OutputInterface tf_; - OutputInterface catapult_pitch_angle_; - OutputInterface catapult_roll_angle_; - OutputInterface catapult_yaw_angle_; - rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; - - std::mutex referee_mutex_; - std::deque referee_ring_buffer_receive_; - OutputInterface referee_serial_; - - int pub_time_count_ = 0; - int force_sensor_frame_count_ = 0; - int can2_unknown_count_ = 0; - - double first_sample_spot_ = 1.0; - double final_sample_spot_ = 4.0; - double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_; - std::vector time_samples_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV3Full, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp deleted file mode 100644 index b0ba0d2d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart_v4.cpp +++ /dev/null @@ -1,449 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "filter/low_pass_filter.hpp" -#include "hardware/device/bmi088.hpp" -#include "hardware/device/can_packet.hpp" -#include "hardware/device/dji_motor.hpp" -#include "hardware/device/lk_motor.hpp" -#include "hardware/device/dr16.hpp" -#include "hardware/device/force_sensor.hpp" -#include "hardware/device/pwm_servo.hpp" -#include "librmcs/agent/c_board.hpp" - -namespace rmcs_core::hardware { - -class CatapultDartV4 - : public rmcs_executor::Component - , public rclcpp::Node - , private librmcs::agent::CBoard { -public: - CatapultDartV4() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} - , dart_command_( - create_partner_component(get_component_name() + "_command", *this)) - , logger_{get_logger()} - , pitch_angle_filter_(20.0, 1000.0) - , pitch_velocity_filter_(20.0, 1000.0) - , yaw_velocity_filter_(20.0, 1000.0) - , yaw_filter_(5.0, 1000.0) - , pitch_filter_(10.0, 1000.0) - , roll_filter_(10.0, 1000.0) - , pitch_motor_{*this, *dart_command_, "/dart/pitch_motor"} - , yaw_motor_{*this, *dart_command_, "/dart/yaw_motor"} - , force_screw_motor_{*this, *dart_command_, "/dart/force_screw"} - , drive_belt_motor_left_{*this, *dart_command_, "/dart/drive_belt/left"} - , drive_belt_motor_right_{*this, *dart_command_, "/dart/drive_belt/right"} - , lifting_left_motor_{*this, *dart_command_, "/dart/lifting_left"} - , lifting_right_motor_{*this, *dart_command_, "/dart/lifting_right"} - , force_sensor_{*this} - , trigger_servo_{"/dart/trigger_servo", *dart_command_, 20.0, 0.5, 2.5} - - , dr16_{*this} - , imu_{1000, 0.2, 0.0} { - - pitch_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - yaw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - force_screw_motor_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_left_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(19.)); - drive_belt_motor_right_.configure( - device::DjiMotor::Config{device::DjiMotor::Type::kM3508} - .set_reversed() - .set_reduction_ratio(19.)); - - first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); - final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); - - setup_imu_coordinate_mapping(); - start_time_ = std::chrono::steady_clock::now(); - yaw_drift_coefficient_ = 0.0; - tf_broadcaster_ = std::make_unique(*this); - - register_output("/tf", tf_); - register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); - - lifting_left_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - lifting_right_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); - - force_sensor_calibrate_subscription_ = create_subscription( - "/force_sensor/calibrate", rclcpp::QoS{0}, - [this](std_msgs::msg::Int32::UniquePtr&& msg) { - if (msg->data == 0) { - RCLCPP_INFO(logger_, "Force sensor zero calibration command received."); - auto board = start_transmit(); - board.can1_transmit({ - .can_id = 0x201, - .can_data = device::CanPacket8{device::ForceSensor::generate_zero_calibration_command()}.as_bytes(), - }); - } else { - RCLCPP_WARN(logger_, "Unknown force sensor calibration command: %d", msg->data); - } - } - ); - - register_output("/referee/serial", referee_serial_); - referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { - std::lock_guard lock(referee_mutex_); - size_t count = 0; - while (count < size && !referee_ring_buffer_receive_.empty()) { - buffer[count++] = referee_ring_buffer_receive_.front(); - referee_ring_buffer_receive_.pop_front(); - } - return count; - }; - referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { - auto board = start_transmit(); - board.uart1_transmit({.uart_data = std::span{buffer, size}}); - return size; - }; - } - - CatapultDartV4(const CatapultDartV4&) = delete; - CatapultDartV4& operator=(const CatapultDartV4&) = delete; - CatapultDartV4(CatapultDartV4&&) = delete; - CatapultDartV4& operator=(CatapultDartV4&&) = delete; - - ~CatapultDartV4() override = default; - - void update() override { - dr16_.update_status(); - pitch_motor_.update_status(); - yaw_motor_.update_status(); - drive_belt_motor_left_.update_status(); - drive_belt_motor_right_.update_status(); - force_screw_motor_.update_status(); - force_sensor_.update_status(); - imu_.update_status(); - processImuData(); - } - - void command_update() { - auto board = start_transmit(); - - board.gpio_analog_transmit({.channel = 1, .value = trigger_servo_.generate_duty_cycle()}); - - if (pub_time_count_++ > 100) { - board.can1_transmit({ - .can_id = 0x301, - .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), - }); - pub_time_count_ = 0; - } - - board.can1_transmit({ - .can_id = 0x141, - .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), - }); - - board.can1_transmit({ - .can_id = 0x145, - .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), - }); - - board.can2_transmit({ - .can_id = 0x200, - .can_data = - device::CanPacket8{ - pitch_motor_.generate_command(), - yaw_motor_.generate_command(), - force_screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - - }); - - board.can2_transmit({ - .can_id = 0x1FF, - .can_data = - device::CanPacket8{ - drive_belt_motor_left_.generate_command(), - drive_belt_motor_right_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), - }); - } - -protected: - void can1_receive_callback(const librmcs::data::CanDataView& data) override { - if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] - return; - const auto can_id = data.can_id; - - if (can_id == 0x302) { - force_sensor_frame_count_++; - if (force_sensor_frame_count_ % 200 == 1) { - RCLCPP_INFO( - logger_, "[ForceSensor CAN2:0x302] raw(%zu): %s", - data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - force_sensor_.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) [[unlikely]] - return; - const auto can_id = data.can_id; - - // Log every unknown CAN2 ID (throttled per ID) to find force sensor response ID - if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 - && can_id != 0x205 && can_id != 0x206) { - can2_unknown_count_++; - if (can2_unknown_count_ % 50 == 1) { - RCLCPP_INFO( - logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", - can_id, data.can_data.size(), - bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); - } - } - - if (can_id == 0x201) { - pitch_motor_.store_status(data.can_data); - } else if (can_id == 0x202) { - yaw_motor_.store_status(data.can_data); - } else if (can_id == 0x203) { - force_screw_motor_.store_status(data.can_data); - } else if (can_id == 0x205) { - drive_belt_motor_left_.store_status(data.can_data); - } else if (can_id == 0x206) { - drive_belt_motor_right_.store_status(data.can_data); - } - } - - void uart1_receive_callback(const librmcs::data::UartDataView& data) override { - std::lock_guard lock(referee_mutex_); - for (auto byte : data.uart_data) { - referee_ring_buffer_receive_.push_back(byte); - } - } - - void uart2_receive_callback(const librmcs::data::UartDataView& data) override { - } - - 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 { - imu_.store_accelerometer_status(data.x, data.y, data.z); - } - - void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { - imu_.store_gyroscope_status(data.x, data.y, data.z); - } - -private: - static std::string bytes_to_hex_string(const std::byte* data, size_t size) { - if (!data || size == 0) - return "[]"; - std::stringstream ss; - ss << std::hex << std::uppercase << std::setfill('0'); - for (size_t i = 0; i < size; ++i) - ss << std::setw(2) << static_cast(data[i]) << " "; - std::string result = ss.str(); - if (!result.empty() && result.back() == ' ') - result.pop_back(); - return result; - } - - void setup_imu_coordinate_mapping() { - imu_.set_coordinate_mapping( - [](double x, double y, double z) -> std::tuple { - return {x, -y, -z}; - }); - } - - void processImuData() { - auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = - std::chrono::duration(current_time - start_time_).count(); - - Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); - Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - - double roll = std::atan2(rot(2, 1), rot(2, 2)); - double pitch = std::asin(-rot(2, 0)); - double yaw = std::atan2(rot(1, 0), rot(0, 0)); - - double t_roll = -roll_filter_.update(roll); - double t_pitch = pitch_filter_.update(pitch); - double t_yaw = -yaw_filter_.update(yaw); - - if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { - yaw_samples_.push_back(t_yaw); - time_samples_.push_back(elapsed_seconds); - - double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; - double n = static_cast(yaw_samples_.size()); - for (size_t i = 0; i < yaw_samples_.size(); ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; - sum_xy += time_samples_[i] * yaw_samples_[i]; - sum_xx += time_samples_[i] * time_samples_[i]; - } - double denominator = n * sum_xx - sum_x * sum_x; - if (std::abs(denominator) > 1e-10) - yaw_drift_coefficient_ = (n * sum_xy - sum_x * sum_y) / denominator; - } - - t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; - publishTfTransforms(t_roll, t_pitch, t_yaw); - - *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; - *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; - } - - void publishTfTransforms(double roll, double pitch, double yaw) { - auto now = this->get_clock()->now(); - - auto make_tf = - [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; - - auto make_vec = [](double x, double y, double z) { - geometry_msgs::msg::Vector3 v; - v.x = x; v.y = y; v.z = z; - return v; - }; - - auto make_quat = [](double x, double y, double z, double w) { - geometry_msgs::msg::Quaternion q; - q.x = x; q.y = y; q.z = z; q.w = w; - return q; - }; - - Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); - Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); - Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - - auto zero = make_vec(0, 0, 0); - auto p_trans = make_vec(0, 0, 0.05); - auto id_rot = make_quat(0, 0, 0, 1); - - tf_broadcaster_->sendTransform( - make_tf("base_link", "gimbal_center_link", zero, id_rot)); - tf_broadcaster_->sendTransform(make_tf( - "gimbal_center_link", "yaw_link", zero, - make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "yaw_link", "pitch_link", p_trans, - make_quat(pitch_q.x(), pitch_q.y(), pitch_q.z(), pitch_q.w()))); - tf_broadcaster_->sendTransform(make_tf( - "pitch_link", "odom_imu", zero, - make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); - tf_broadcaster_->sendTransform( - make_tf("world", "base_link", zero, id_rot)); - } - - class DartCommand : public rmcs_executor::Component { - public: - explicit DartCommand(CatapultDartV4& dart) - : dart_(dart) {} - void update() override { dart_.command_update();} - - private: - CatapultDartV4& dart_; - }; - - std::shared_ptr dart_command_; - rclcpp::Logger logger_; - - filter::LowPassFilter<1> pitch_angle_filter_; - filter::LowPassFilter<1> pitch_velocity_filter_; - filter::LowPassFilter<1> yaw_velocity_filter_; - filter::LowPassFilter<1> yaw_filter_; - filter::LowPassFilter<1> pitch_filter_; - filter::LowPassFilter<1> roll_filter_; - - std::chrono::steady_clock::time_point start_time_; - - device::DjiMotor pitch_motor_; - device::DjiMotor yaw_motor_; - device::DjiMotor force_screw_motor_; - device::DjiMotor drive_belt_motor_left_; - device::DjiMotor drive_belt_motor_right_; - - device::LkMotor lifting_left_motor_; - device::LkMotor lifting_right_motor_; - - device::ForceSensor force_sensor_; - - device::PWMServo trigger_servo_; - - device::Dr16 dr16_; - device::Bmi088 imu_; - - std::unique_ptr tf_broadcaster_; - - OutputInterface tf_; - OutputInterface catapult_pitch_angle_; - OutputInterface catapult_roll_angle_; - OutputInterface catapult_yaw_angle_; - rclcpp::Subscription::SharedPtr force_sensor_calibrate_subscription_; - - std::mutex referee_mutex_; - std::deque referee_ring_buffer_receive_; - OutputInterface referee_serial_; - - int pub_time_count_ = 0; - int force_sensor_frame_count_ = 0; - int can2_unknown_count_ = 0; - - double first_sample_spot_ = 1.0; - double final_sample_spot_ = 4.0; - double yaw_drift_coefficient_ = 0.0; - std::vector yaw_samples_; - std::vector time_samples_; -}; - -} // namespace rmcs_core::hardware - -#include -PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::CatapultDartV4, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp b/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp deleted file mode 100644 index 3d8d446d..00000000 --- a/rmcs_ws/src/rmcs_core/src/hardware/dart_filling_test_hardware.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// #include -// #include - -// #include "hardware/device/can_packet.hpp" -// #include "hardware/device/dji_motor.hpp" -// #include "hardware/device/force_sensor.hpp" -// #include "hardware/device/pwm_servo.hpp" -// #include "librmcs/agent/c_board.hpp" -// #include "hardware/device/dr16.hpp" -// #include "hardware/device/trigger_servo.hpp" - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// namespace rmcs_core::hardware { - -// class DartFillingTestHardware -// : public rmcs_executor::Component -// , public rclcpp::Node -// , private librmcs::agent::CBoard { -// public: -// DartFillingTestHardware() -// : Node{ -// get_component_name(), -// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} -// , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} -// , logger_{get_logger()} -// , dart_command_(create_partner_component(get_component_name() + "_command",*this)) -// , dr16_(*this) -// , limiting_servo_(dart_command_, "/dart/limiting_servo",LIMITING_SERVO_ID) -// , lifting_left_(*this, *this->dart_command_, "/dart/lifting_left", LIFTING_LEFT_ID) -// , lifting_right_(*this, *this->dart_command_, "/dart/lifting_right", LIFTING_RIGHT_ID) -// , left_belt_motor_{*this, *this->dart_command_, "/dart/left_belt_motor"} -// , right_belt_motor_{*this, *this->dart_command_, "/dart/right_belt_motor"} -// , yaw_control_motor_{*this, *this->dart_command_, "/dart/yaw_control_motor"} -// , pitch_control_motor_{*this, *this->dart_command_, "/dart/pitch_control_motor"} -// , screw_motor_{*this, *this->dart_command_, "/dart/screw_motor"} -// , trigger_servo_{"/dart/trigger_servo", *this->dart_command_, 20.0, 0.5, 2.5} -// , force_sensor_{*this} -// // , transmit_buffer_(*this, 32) -// // , event_thread_([this]() { handle_events(); }) -// { -// register_output("/dart/lifting_left/current_angle", lifting_current_angle_left_); -// register_output("/dart/lifting_right/current_angle", lifting_current_angle_right_); - -// limiting_calibrate_subscription_ = create_subscription( -// "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) -// { -// trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg), -// limiting_servo_uart_data_ptr); -// }); -// lifting_left_calibrate_subscription_ = create_subscription( -// "/lifting_left/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& -// msg) { -// trigger_servo_calibrate_subscription_callback(lifting_left_, std::move(msg), -// lifting_left_uart_data_ptr); -// }); -// lifting_right_calibrate_subscription_ = create_subscription( -// "/lifting_right/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& -// msg) { -// trigger_servo_calibrate_subscription_callback(lifting_right_, std::move(msg), -// lifting_right_uart_data_ptr); -// }); - -// 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; -// // }; - -// last_read_left_time_ = this->now(); -// last_read_right_time_ = this->now(); -// } - -// ~DartFillingTestHardware() override { -// // stop_handling_events(); -// event_thread_.join(); -// } - -// void update() override { -// dr16_.update_status(); -// } - -// void command_update() { - -// if (!trigger_servo_.calibrate_mode()) { -// uint16_t current_target = trigger_servo_.get_target_angle(); -// if (current_target != last_trigger_angle_) { -// size_t uart_data_length; -// std::unique_ptr command_buffer = -// trigger_servo_.generate_runtime_command(uart_data_length); -// const auto trigger_servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(trigger_servo_uart_data_ptr, -// uart_data_length); last_trigger_angle_ = current_target; -// } -// } - -// if (!limiting_servo_.calibrate_mode()) { -// uint16_t current_target = limiting_servo_.get_target_angle(); -// if (current_target != last_limiting_angle_) { -// size_t uart_data_length; -// auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_limiting_angle_ = current_target; -// } -// } - -// if (!lifting_left_.calibrate_mode() && !lifting_right_.calibrate_mode()) { -// uint16_t current_target_left = lifting_left_.get_target_angle(); -// uint16_t current_target_right = lifting_right_.get_target_angle(); -// if (current_target_left != last_lifting_left_angle_) { -// size_t uart_data_length; -// uint16_t runtime_left = 0; -// uint16_t runtime_right = 0; -// std::unique_ptr command_buffer = -// device::TriggerServo::generate_sync_run_command(uart_data_length, -// LIFTING_LEFT_ID, -// LIFTING_RIGHT_ID, -// current_target_left, -// current_target_right, -// runtime_left, runtime_right); -// const auto lifting_table_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(lifting_table_uart_data_ptr, -// uart_data_length); last_lifting_left_angle_ = current_target_left; -// } -// } - -// auto now = this->now(); - -// if (!lifting_left_.calibrate_mode() && (now - last_read_left_time_) >= read_interval_) { -// size_t uart_data_length; -// auto command_buffer = lifting_left_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_left_time_ = now; -// } -// if (!lifting_right_.calibrate_mode() && (now - last_read_right_time_) >= read_interval_) -// { -// if ((now - last_read_left_time_) >= rclcpp::Duration::from_seconds(0.01)) { -// size_t uart_data_length; -// auto command_buffer = lifting_right_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, -// uart_data_length); -// transmit_buffer_.add_uart2_transmission(command_buffer.get(), uart_data_length); -// last_read_right_time_ = now; -// } -// } - -// transmit_buffer_.trigger_transmission(); -// } - -// int pub_time_count_ = 0; - -// protected: -// void uart1_receive_callback(const std::byte* data, uint8_t length) override { -// referee_ring_buffer_receive_.emplace_back_multi( -// [&data](std::byte* storage) { *storage = *data++; }, length); -// } - -// void uart2_receive_callback(const std::byte* data, uint8_t length) override { -// std::string hex_string = bytes_to_hex_string(data, length); -// RCLCPP_DEBUG(this->get_logger(), "UART2 received: len=%d [%s]", length, -// hex_string.c_str()); - -// if (length < 3) { -// RCLCPP_WARN(logger_, "UART2 data too short: %d bytes", length); -// return; -// } - -// uint8_t servo_id = static_cast(data[2]); -// std::pair result{false, 0}; - -// switch (servo_id) { -// case TRIGGER_SERVO_ID: -// result = trigger_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIMITING_SERVO_ID: -// result = limiting_servo_.calibrate_current_angle(logger_, data, length); -// if (!result.first) { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_LEFT_ID: -// result = lifting_left_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_left_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// case LIFTING_RIGHT_ID: -// result = lifting_right_.calibrate_current_angle(logger_, data, length); -// if (result.first) { -// *lifting_current_angle_right_ = result.second; -// } else { -// RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// } -// break; -// default: RCLCPP_INFO(logger_, "calibrate: uart2 data store failed"); -// break; -// } - -// } - -// void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { -// dr16_.store_status(uart_data, uart_data_length); -// } - -// private: -// static constexpr uint8_t TRIGGER_SERVO_ID = 0x01; -// static constexpr uint8_t LIMITING_SERVO_ID = 0x02; -// static constexpr uint8_t LIFTING_LEFT_ID = 0x03; -// static constexpr uint8_t LIFTING_RIGHT_ID = 0x04; -// std::byte * trigger_servo_uart_data_ptr; -// std::byte * limiting_servo_uart_data_ptr; -// std::byte * lifting_left_uart_data_ptr; -// std::byte * lifting_right_uart_data_ptr; - -// void trigger_servo_calibrate_subscription_callback(device::TriggerServo& servo_ -// , std_msgs::msg::Int32::UniquePtr msg -// , std::byte* servo_uart_data_ptr) { -// /* -// 标定命令格式： -// ros2 topic pub --rate 2 --times 5 /trigger/calibrate std_msgs/msg/Int32 "{'data':0}" -// 替换data值就行 -// */ -// servo_.set_calibrate_mode(msg->data); - -// std::unique_ptr command_buffer; -// size_t command_length = 0; -// if (msg->data == 0) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_SERVO_MODE, command_length); -// } else if (msg->data == 1) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::SWITCH_TO_MOTOR_MODE, command_length); -// } else if (msg->data == 2) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_FORWARD_MODE, command_length); -// } else if (msg->data == 3) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_REVERSE_MODE, command_length); -// } else if (msg->data == 4) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_RUNTIME_CONTROL, command_length); -// } else if (msg->data == 5) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::MOTOR_DISABLE_CONTROL, command_length); -// } else if (msg->data == 6) { -// command_buffer = servo_.generate_calibrate_command( -// device::TriggerServo::CalibrateOperation::READ_CURRENT_ANGLE, command_length); -// } - -// servo_uart_data_ptr = command_buffer.get(); -// transmit_buffer_.add_uart2_transmission(servo_uart_data_ptr, command_length); - -// std::string hex_string = bytes_to_hex_string(command_buffer.get(), command_length); -// RCLCPP_INFO( -// this->get_logger(), "UART2 Pub: (length=%zu)[ %s ]", command_length, -// hex_string.c_str()); -// } - -// static std::string bytes_to_hex_string(const std::byte* data, size_t size) { -// if (!data || size == 0) { -// return "[]"; -// } - -// std::stringstream ss; -// ss << std::hex << std::uppercase << std::setfill('0'); - -// for (size_t i = 0; i < size; ++i) { -// ss << std::setw(2) << static_cast(data[i]) << " "; -// } - -// std::string result = ss.str(); -// if (!result.empty() && result.back() == ' ') { -// result.pop_back(); -// } -// return result; -// } // DEBUG TOOL - -// rclcpp::Logger logger_; -// class DartCommand : public rmcs_executor::Component { -// public: -// explicit DartCommand(DartFillingTestHardware& robot) : dart_(robot) {} -// void update() override { dart_.command_update(); } -// DartFillingTestHardware& dart_; -// }; - -// std::shared_ptr dart_command_; -// device::Dr16 dr16_; -// device::TriggerServo limiting_servo_; -// device::TriggerServo lifting_left_; -// device::TriggerServo lifting_right_; -// device::DjiMotor left_belt_motor_, right_belt_motor_; -// device::DjiMotor yaw_control_motor_, pitch_control_motor_; -// device::DjiMotor screw_motor_; -// device::PWMServo trigger_servo_; -// device::ForceSensor force_sensor_; -// rclcpp::Subscription::SharedPtr trigger_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_left_calibrate_subscription_; -// rclcpp::Subscription::SharedPtr lifting_right_calibrate_subscription_; -// OutputInterface referee_serial_; -// std::thread event_thread_; -// OutputInterface lifting_current_angle_left_; -// OutputInterface lifting_current_angle_right_; -// std::vector yaw_samples_, time_samples_; -// uint16_t last_trigger_angle_ = 0xFFFF; -// uint16_t last_limiting_angle_ = 0xFFFF; -// uint16_t last_lifting_left_angle_ = 0xFFFF; -// rclcpp::Time last_read_left_time_; -// rclcpp::Time last_read_right_time_; -// const rclcpp::Duration read_interval_ = rclcpp::Duration::from_seconds(0.5); -// }; -// } // namespace rmcs_core::hardware - -// #include -// PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::DartFillingTestHardware, rmcs_executor::Component) \ No newline at end of file From 0105ab3210c894b4dec1a4a9bd24d309224e23a1 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Tue, 17 Mar 2026 09:24:07 +0800 Subject: [PATCH 41/45] Stable version --- .gitmodules | 3 - .../rmcs_bringup/config/dart-guidance.yaml | 24 +-- rmcs_ws/src/rmcs_core/plugins.xml | 6 +- .../rmcs_core/src/controller/dart/filling.cpp | 75 ++++++++ .../src/controller/dart/launch_setting.cpp | 162 ++++++++---------- .../rmcs_core/src/hardware/catapult_dart.cpp | 4 +- rmcs_ws/src/rmcs_dart_guidance | 2 +- .../rmcs_msgs/dart_limiting_servo_status.hpp | 11 ++ 8 files changed, 179 insertions(+), 108 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/dart/filling.cpp create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_limiting_servo_status.hpp diff --git a/.gitmodules b/.gitmodules index 5ad90e84..d4d71be2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -10,6 +10,3 @@ [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git -[submodule "rmcs_ws/src/rmcs_dart_guidance"] - path = rmcs_ws/src/rmcs_dart_guidance - url = https://github.com/hyperle/rmcs_submodule_dart_guidance.git diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 765b94d4..52e25f48 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -7,6 +7,7 @@ rmcs_executor: - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting + - rmcs_core::controller::dart::DartFilling -> dart_filling - rmcs_core::controller::dart::DartSettingController -> dart_setting - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster @@ -21,30 +22,29 @@ dart_manager: ros__parameters: max_transform_rate: 5.0 manual_force_scale: 5.0 - limiting_open_angle: 500 # uint16_t，需实测标定 - limiting_close_angle: 1000 # uint16_t，需实测标定 - limiting_fill_ticks: 500 + limiting_fill_ticks: 1000 lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 lifting_stall_timeout_ticks: 2000 # 超时帧数 = 5s -remote_cmd_bridge: - ros__parameters: - joystick_sensitivity: 0.01 - launch_setting: ros__parameters: - belt_velocity: 10.0 + belt_velocity: 10.0 # manager 未提供目标速度时的回退值 sync_coefficient: 0.1 max_control_torque: 2.5 trigger_free_value: 0.75 trigger_lock_value: 0.63 - b_kp: 5.0 + b_kp: 1.0 b_ki: 0.0 b_kd: 0.0 - lifting_velocity: 10.0 # rad/s，正值 = 左+右-（平台上升方向），需实测标定方向 - belt_hold_torque: 15.0 # Nm，传送带下方堵转后的保持扭矩，需实测标定 + belt_hold_torque: 15.0 # manager 未提供保留力矩时的回退值 + +dart_filling: + ros__parameters: + lifting_velocity: 3.0 + limiting_free_angle: 500 + limiting_lock_angle: 1000 dart_setting: ros__parameters: @@ -68,4 +68,4 @@ broadcaster: ros__parameters: forward_list: - /dart/drive_belt/left/control_torque - - /dart/drive_belt/right/control_torque \ No newline at end of file + - /dart/drive_belt/right/control_torque diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 2c350ee4..51ba3ca0 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -153,12 +153,12 @@ Dart setting controller: subscribes to RemoteCommandBridge delta commands, outputs yaw/pitch/force control velocities. - Launch setting V2: belt + trigger + LK lifting angle control. + Launch setting V2: belt + trigger execution with task-level velocity, torque limit, and hold torque. - Launch setting V2: belt + trigger + LK lifting angle control. + Dart filling execution: maps manager lifting and limiting commands to LK motor velocity and calibrated servo angles. Launch setting V2: belt + trigger + LK lifting angle control. - \ No newline at end of file + diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/filling.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/filling.cpp new file mode 100644 index 00000000..9f266622 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/filling.cpp @@ -0,0 +1,75 @@ +#include + +#include +#include + +#include +#include + +namespace rmcs_core::controller::dart { + +class DartFilling + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartFilling() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , lifting_velocity_(get_parameter("lifting_velocity").as_double()) + , limiting_free_angle_(static_cast(get_parameter("limiting_free_angle").as_int())) + , limiting_lock_angle_( + static_cast(get_parameter("limiting_lock_angle").as_int())) { + register_input("/dart/manager/lifting/command", lifting_command_); + register_input("/dart/manager/limiting/command", limiting_command_); + + register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); + register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); + register_output( + "/dart/limiting_servo/control_angle", limiting_servo_angle_, limiting_lock_angle_); + } + + void update() override { + switch (*lifting_command_) { + case rmcs_msgs::DartSliderStatus::UP: + *lifting_left_vel_ = -lifting_velocity_; + *lifting_right_vel_ = +lifting_velocity_; + break; + case rmcs_msgs::DartSliderStatus::DOWN: + *lifting_left_vel_ = +lifting_velocity_; + *lifting_right_vel_ = -lifting_velocity_; + break; + default: + *lifting_left_vel_ = 0.0; + *lifting_right_vel_ = 0.0; + break; + } + + switch (*limiting_command_) { + case rmcs_msgs::DartLimitingServoStatus::FREE: + *limiting_servo_angle_ = limiting_free_angle_; + break; + case rmcs_msgs::DartLimitingServoStatus::LOCK: + *limiting_servo_angle_ = limiting_lock_angle_; + break; + case rmcs_msgs::DartLimitingServoStatus::WAIT: break; + } + } + +private: + double lifting_velocity_; + uint16_t limiting_free_angle_; + uint16_t limiting_lock_angle_; + + InputInterface lifting_command_; + InputInterface limiting_command_; + + OutputInterface lifting_left_vel_; + OutputInterface lifting_right_vel_; + OutputInterface limiting_servo_angle_; +}; + +} // namespace rmcs_core::controller::dart + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartFilling, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp index 7309e59f..6f78f191 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -2,8 +2,6 @@ #include #include -#include -#include #include #include #include @@ -12,17 +10,11 @@ namespace rmcs_core::controller::dart { -// DartLaunchSettingV2 — DartLaunchSetting 的升级版，新增 LK 升降电机速度控制。 -// -// 升降控制逻辑（仅做命令→速度映射，无堵转检测）： -// UP → left = +lifting_velocity, right = -lifting_velocity -// DOWN → left = -lifting_velocity, right = +lifting_velocity -// WAIT → left = right = 0（电机以小电流维持当前位置） -// -// 堵转检测由 LiftingLkAction 内部完成（直接读速度反馈），不在此处处理。 -// -// yaml 新增参数： -// lifting_velocity (double, rad/s) — 升降速度，正值=左+右-（平台上升方向），需实测标定 +// DartLaunchSettingV2 +// 同步带与扳机执行组件： +// - 速度、限扭、保留力矩由上层 DartManagerV2 下发 +// - 本组件仅负责 belt PID 同步控制和扳机量值映射 +// - 升降机构与限位舵机由 DartFilling 独立处理 class DartLaunchSettingV2 : public rmcs_executor::Component , public rclcpp::Node { @@ -30,100 +22,100 @@ class DartLaunchSettingV2 DartLaunchSettingV2() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , logger_(get_logger()) , belt_pid_{ get_parameter("b_kp").as_double(), get_parameter("b_ki").as_double(), get_parameter("b_kd").as_double()} { - belt_velocity_ = get_parameter("belt_velocity").as_double(); - sync_coefficient_ = get_parameter("sync_coefficient").as_double(); - max_control_torque_ = get_parameter("max_control_torque").as_double(); - trigger_free_value_ = get_parameter("trigger_free_value").as_double(); - trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); - lifting_velocity_ = get_parameter("lifting_velocity").as_double(); - belt_hold_torque_ = get_parameter("belt_hold_torque").as_double(); - - register_input("/dart/manager/belt/command", belt_command_); + belt_velocity_ = get_parameter("belt_velocity").as_double(); + sync_coefficient_ = get_parameter("sync_coefficient").as_double(); + max_control_torque_ = get_parameter("max_control_torque").as_double(); + trigger_free_value_ = get_parameter("trigger_free_value").as_double(); + trigger_lock_value_ = get_parameter("trigger_lock_value").as_double(); + default_belt_hold_torque_ = get_parameter("belt_hold_torque").as_double(); + + register_input("/dart/manager/belt/command", belt_command_); + register_input("/dart/manager/belt/target_velocity", belt_target_velocity_, false); + register_input("/dart/manager/belt/torque_limit", belt_torque_limit_, false); + register_input("/dart/manager/belt/hold_torque", belt_hold_torque_input_, false); + register_input("/dart/manager/belt/wait_zero_velocity", belt_wait_zero_velocity_, false); register_input("/dart/manager/trigger/lock_enable", trigger_lock_enable_); - register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); - register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); - register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); - register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); - register_input("/dart/manager/lifting/command", lifting_command_); - // 第一发下降速度缩放因子（shot_count==1 时 0.8，其余 1.0） - register_input("/dart/manager/belt/down_scale", belt_down_scale_); - // 归零模式：true 时将传送带扭矩上限限制到 10%，防止顶住机械限位过热 - register_input("/dart/manager/belt/homing", belt_homing_mode_); + register_input("/dart/drive_belt/left/velocity", left_belt_velocity_); + register_input("/dart/drive_belt/right/velocity", right_belt_velocity_); + register_input("/force_sensor/channel_1/weight", force_sensor_ch1_); + register_input("/force_sensor/channel_2/weight", force_sensor_ch2_); register_output( - "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); + "/dart/drive_belt/left/control_torque", left_belt_torque_, 0.0); register_output( "/dart/drive_belt/right/control_torque", right_belt_torque_, 0.0); register_output("/dart/trigger_servo/value", trigger_value_, trigger_lock_value_); - register_output("/dart/lifting_left/control_velocity", lifting_left_vel_, 0.0); - register_output("/dart/lifting_right/control_velocity", lifting_right_vel_, 0.0); } void update() override { + const double requested_velocity = + belt_target_velocity_.ready() ? std::abs(*belt_target_velocity_) : belt_velocity_; + + BeltControlMode control_mode = BeltControlMode::WAIT_HOLD; double control_velocity = 0.0; switch (*belt_command_) { case rmcs_msgs::DartSliderStatus::DOWN: - control_velocity = +belt_velocity_ * (*belt_down_scale_); - prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; + control_mode = BeltControlMode::MOVE_DOWN; + control_velocity = +requested_velocity; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; break; case rmcs_msgs::DartSliderStatus::UP: - control_velocity = -belt_velocity_; - prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; + control_mode = BeltControlMode::MOVE_UP; + control_velocity = -requested_velocity; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; break; default: - control_velocity = 0.0; + control_mode = + belt_wait_zero_velocity_.ready() && *belt_wait_zero_velocity_ + ? BeltControlMode::WAIT_ZERO + : BeltControlMode::WAIT_HOLD; break; } - double torque_limit = *belt_homing_mode_ - ? max_control_torque_ * 0.1 - : max_control_torque_; - drive_belt_sync_control(control_velocity, torque_limit); - *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; - // RCLCPP_INFO(logger_,"ch1: %d | ch2: %d",*force_sensor_ch1_,*force_sensor_ch2_); + if (control_mode != control_mode_) { + belt_pid_.reset(); + control_mode_ = control_mode; + } - // 升降电机速度控制（UP/DOWN/WAIT → 速度映射） - switch (*lifting_command_) { - case rmcs_msgs::DartSliderStatus::UP: - *lifting_left_vel_ = +lifting_velocity_; - *lifting_right_vel_ = -lifting_velocity_; - break; - case rmcs_msgs::DartSliderStatus::DOWN: - *lifting_left_vel_ = -lifting_velocity_; - *lifting_right_vel_ = +lifting_velocity_; - break; - default: - *lifting_left_vel_ = 0.0; - *lifting_right_vel_ = 0.0; - break; + double torque_limit = + belt_torque_limit_.ready() ? std::abs(*belt_torque_limit_) : max_control_torque_; + torque_limit = std::min(torque_limit, max_control_torque_); + + if (control_mode == BeltControlMode::WAIT_HOLD) { + apply_hold_torque(); + } else { + drive_belt_sync_control(control_velocity, torque_limit); } + + *trigger_value_ = *trigger_lock_enable_ ? trigger_lock_value_ : trigger_free_value_; } private: - void drive_belt_sync_control(double set_velocity, double torque_limit) { - // WAIT 状态：清除 PID 积分，避免残余扭矩 - // 上端（prev=UP）：完全清零，避免顶着限位持续出力 - // 下端（prev=DOWN）：施加保持扭矩，防止重力/弹力导致滑台回弹 - if (set_velocity == 0.0) { - belt_pid_.reset(); - if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { - *left_belt_torque_ = +belt_hold_torque_; - *right_belt_torque_ = +belt_hold_torque_; - RCLCPP_INFO(logger_, "belt_combating"); - } else { - *left_belt_torque_ = 0.0; - *right_belt_torque_ = 0.0; - RCLCPP_INFO(logger_, "belt_init done"); - } - return; + enum class BeltControlMode { + MOVE_UP, + MOVE_DOWN, + WAIT_ZERO, + WAIT_HOLD, + }; + + void apply_hold_torque() { + double hold_torque = 0.0; + if (belt_hold_torque_input_.ready()) { + hold_torque = *belt_hold_torque_input_; + } else if (prev_belt_cmd_ == rmcs_msgs::DartSliderStatus::DOWN) { + hold_torque = default_belt_hold_torque_; } + *left_belt_torque_ = hold_torque; + *right_belt_torque_ = hold_torque; + } + + void drive_belt_sync_control(double set_velocity, double torque_limit) { Eigen::Vector2d setpoint_error{ set_velocity - *left_belt_velocity_, set_velocity - *right_belt_velocity_}; @@ -138,8 +130,6 @@ class DartLaunchSettingV2 *right_belt_torque_ = std::clamp(control_torques[1], -torque_limit, torque_limit); } - rclcpp::Logger logger_; - pid::MatrixPidCalculator<2> belt_pid_; double belt_velocity_; @@ -147,30 +137,28 @@ class DartLaunchSettingV2 double max_control_torque_; double trigger_free_value_; double trigger_lock_value_; - double lifting_velocity_; - double belt_hold_torque_{0.0}; + double default_belt_hold_torque_{0.0}; rmcs_msgs::DartSliderStatus prev_belt_cmd_{rmcs_msgs::DartSliderStatus::WAIT}; + BeltControlMode control_mode_{BeltControlMode::WAIT_HOLD}; InputInterface belt_command_; + InputInterface belt_target_velocity_; + InputInterface belt_torque_limit_; + InputInterface belt_hold_torque_input_; + InputInterface belt_wait_zero_velocity_; InputInterface trigger_lock_enable_; InputInterface left_belt_velocity_; InputInterface right_belt_velocity_; InputInterface force_sensor_ch1_, force_sensor_ch2_; - InputInterface lifting_command_; - InputInterface belt_down_scale_; - InputInterface belt_homing_mode_; - OutputInterface left_belt_torque_; OutputInterface right_belt_torque_; OutputInterface trigger_value_; - OutputInterface lifting_left_vel_; - OutputInterface lifting_right_vel_; }; } // namespace rmcs_core::controller::dart #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::dart::DartLaunchSettingV2, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 3340531c..eca43538 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -36,12 +36,12 @@ /* CatapultDartV3Lk — catapult_dart_v3_full 的变体 -升降电机替换为瓴控4005 (LkMotor MG4005Ei10)，挂 CAN1 (0x141 左, 0x142 右)。 +升降电机替换为瓴控4005 (LkMotor MG4005Ei10)，挂 CAN1 (0x141 左, 0x145 右)。 限位舵机保留 TriggerServo (UART2, ID=0x03)。 升降电机接口 (double, rad): 输出: /dart/lifting_left/angle, /dart/lifting_left/velocity 等 - 输入: /dart/lifting_left/control_angle (由 launch_controller_v2_full 写入) + 输入: /dart/lifting_left/control_velocity (由 DartFilling 写入) */ namespace rmcs_core::hardware { diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index db6b7f20..f563f2e3 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit db6b7f20d52313c23cb4bd64446c8281d5137b39 +Subproject commit f563f2e32ef7e51839d56be9609bf82262cd0595 diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_limiting_servo_status.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_limiting_servo_status.hpp new file mode 100644 index 00000000..8161c3d9 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_limiting_servo_status.hpp @@ -0,0 +1,11 @@ +#pragma once + +namespace rmcs_msgs { + +enum class DartLimitingServoStatus { + FREE = 0, + LOCK = 1, + WAIT = 2, +}; + +} // namespace rmcs_msgs From b4d20c3fb68c91d530820ffc2f1b835763aae2f1 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 18 Mar 2026 15:21:48 +0800 Subject: [PATCH 42/45] Vison dev --- .../rmcs_bringup/config/dart-guidance.yaml | 44 +++++++++++++++++++ .../src/controller/dart/dart_setting.cpp | 19 +++++++- .../src/controller/dart/launch_setting.cpp | 1 - .../rmcs_core/src/hardware/catapult_dart.cpp | 2 +- rmcs_ws/src/rmcs_dart_guidance | 2 +- 5 files changed, 64 insertions(+), 4 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 52e25f48..924bb911 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -3,6 +3,9 @@ rmcs_executor: update_rate: 1000.0 components: - rmcs_core::hardware::CatapultDartV3Lk -> dart_hardware + - rmcs_dart_guidance::DartVisionCore -> dart_vision + - rmcs_dart_guidance::ImagePublisher -> dart_camera_image_publisher + - rmcs_dart_guidance::ImagePublisher -> dart_display_image_publisher - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge @@ -18,10 +21,50 @@ dart_hardware: first_sample_spot: 1.0 final_sample_spot: 4.0 +dart_vision: + ros__parameters: + invert_image: true + exposure_time: 3000 + gain: 11.0 + L_H: 0.0 + L_S: 0.0 + L_V: 200.0 + U_H: 180.0 + U_S: 30.0 + U_V: 255.0 + enable_image_saving: false + image_save_directory: "./saved_images" + image_save_interval_ms: 1000 + save_raw_image: true + save_processed_image: false + +dart_camera_image_publisher: + ros__parameters: + Interface_name: /dart_guidance/camera/camera_image + topic_name: /dart_guidance/camera/camera_image + publish_freq: 60.0 + image_type: bgr8 + +dart_display_image_publisher: + ros__parameters: + Interface_name: /dart_guidance/camera/display_image + topic_name: /dart_guidance/camera/display_image + publish_freq: 60.0 + image_type: mono8 + dart_manager: ros__parameters: max_transform_rate: 5.0 + auto_aim_max_transform_rate: 5.0 manual_force_scale: 5.0 + dart_count: 4 + aim_reference_pixel: [701.0, 565.0] + aim_dart_offsets_px: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + aim_yaw_gain: 0.05 + aim_pitch_gain: 0.05 + aim_deadband_px: [3.0, 3.0] + aim_ready_confirm_ticks: 20 + aim_timeout_ticks: 5000 limiting_fill_ticks: 1000 lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz @@ -54,6 +97,7 @@ dart_setting: pitch_kp: 1.0 pitch_ki: 0.0 pitch_kd: 0.0 + log_enable: true force_screw_velocity_pid_controller: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp index e8443491..ea7df39a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp @@ -28,16 +28,29 @@ class DartSettingController register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_); register_input("/dart/yaw_motor/velocity", yaw_velocity_); register_input("/dart/pitch_motor/velocity", pitch_velocity_); + register_input("/force/control/velocity", force_vel_setpoint_); register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); - + + try { + log_enable_ = get_parameter("log_enable").as_bool(); + } catch (...) { + log_enable_ = false; + } } void update() override { const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_; *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_); + + if (log_enable_) { + if (log_counter_++ >= 1000) { + RCLCPP_INFO(get_logger(), "Manual force control velocity: %.3f", *force_vel_setpoint_); + log_counter_ = 0; + } + } } private: @@ -47,9 +60,13 @@ class DartSettingController InputInterface yaw_pitch_vel_setpoint_; InputInterface yaw_velocity_; InputInterface pitch_velocity_; + InputInterface force_vel_setpoint_; OutputInterface yaw_torque_; OutputInterface pitch_torque_; + + bool log_enable_ = false; + uint32_t log_counter_ = 0; }; } // namespace rmcs_core::controller::dart diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp index 6f78f191..3faedc4d 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -84,7 +84,6 @@ class DartLaunchSettingV2 double torque_limit = belt_torque_limit_.ready() ? std::abs(*belt_torque_limit_) : max_control_torque_; - torque_limit = std::min(torque_limit, max_control_torque_); if (control_mode == BeltControlMode::WAIT_HOLD) { apply_hold_torque(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index eca43538..99d1122b 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -163,7 +163,7 @@ class CatapultDartV3Lk if (pub_time_count_++ > 100) { board.can1_transmit({ .can_id = 0x301, - .can_data = device::CanPacket8::PaddingQuarter{}.as_bytes(), + .can_data = device::CanPacket8{0}.as_bytes(), }); pub_time_count_ = 0; } diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance index f563f2e3..26f3e1f9 160000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ b/rmcs_ws/src/rmcs_dart_guidance @@ -1 +1 @@ -Subproject commit f563f2e32ef7e51839d56be9609bf82262cd0595 +Subproject commit 26f3e1f942dc05a1db7f3d2830f4d4865c32d3ce From 74b1e354e0264272f7f1ef02899b13898899dc56 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 18 Mar 2026 17:59:26 +0800 Subject: [PATCH 43/45] Configure dart guidance for auto aim testing --- .../rmcs_bringup/config/dart-guidance.yaml | 45 +++++++++++++++---- 1 file changed, 36 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index 924bb911..efa82695 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -6,14 +6,15 @@ rmcs_executor: - rmcs_dart_guidance::DartVisionCore -> dart_vision - rmcs_dart_guidance::ImagePublisher -> dart_camera_image_publisher - rmcs_dart_guidance::ImagePublisher -> dart_display_image_publisher - - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager - - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + - rmcs_dart_guidance::manager::AutoAimOnlyComponent -> auto_aim_only + # - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager + # - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge - - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting - - rmcs_core::controller::dart::DartFilling -> dart_filling + # - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting + # - rmcs_core::controller::dart::DartFilling -> dart_filling - rmcs_core::controller::dart::DartSettingController -> dart_setting - - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + # - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + # - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster dart_hardware: ros__parameters: @@ -52,19 +53,45 @@ dart_display_image_publisher: publish_freq: 60.0 image_type: mono8 +auto_aim_only: + ros__parameters: + auto_start: true + success_gate: false + restart_on_terminal: false + target_position_interface: /dart_guidance/camera/target_position + tracking_interface: /dart_guidance/tracker/tracking + control_velocity_interface: /pitch/control/velocity + aim_ready_interface: /dart/manager/aim/ready + aim_error_interface: /dart/manager/aim/error_px + desired_target_interface: /dart/manager/aim/desired_target_px + state_interface: /dart/auto_aim_test/state + desired_target_px: [701.0, 565.0] + aim_yaw_gain: 0.08 + aim_pitch_gain: 0.08 + aim_deadband_px: [3.0, 3.0] + aim_ready_exit_deadband_px: [5.0, 5.0] + aim_ready_confirm_ticks: 20 + aim_timeout_ticks: 5000 + aim_timeout_returns_success: true + aim_min_transform_rate: 1.0 + auto_aim_max_transform_rate: 10.0 + dart_manager: ros__parameters: max_transform_rate: 5.0 - auto_aim_max_transform_rate: 5.0 + auto_aim_max_transform_rate: 10.0 manual_force_scale: 5.0 dart_count: 4 aim_reference_pixel: [701.0, 565.0] aim_dart_offsets_px: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - aim_yaw_gain: 0.05 - aim_pitch_gain: 0.05 + aim_yaw_gain: 0.08 + aim_pitch_gain: 0.08 aim_deadband_px: [3.0, 3.0] + aim_ready_exit_deadband_px: [5.0, 5.0] aim_ready_confirm_ticks: 20 aim_timeout_ticks: 5000 + aim_timeout_returns_success: true + aim_min_transform_rate: 1.0 limiting_fill_ticks: 1000 lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz From 2b2b753406ac8aea555bf0f2e0309237b4585f20 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 18 Mar 2026 22:06:30 +0800 Subject: [PATCH 44/45] Update dart integration and detach rmcs_dart_guidance --- .devcontainer/devcontainer.json | 2 +- .gitignore | 5 +- .../rmcs_bringup/config/dart-guidance.yaml | 28 +-- .../src/controller/dart/dart_setting.cpp | 23 +- .../rmcs_core/src/hardware/catapult_dart.cpp | 215 ++++++++++-------- rmcs_ws/src/rmcs_dart_guidance | 1 - 6 files changed, 150 insertions(+), 124 deletions(-) delete mode 160000 rmcs_ws/src/rmcs_dart_guidance diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 42f5b137..b472dec4 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile { "name": "rmcs-develop", - "image": "qzhhhi/rmcs-develop:latest", + "image": "qzhhhi/rmcs- :latest", "privileged": true, "mounts": [ { diff --git a/.gitignore b/.gitignore index 80c67b8a..667ababa 100644 --- a/.gitignore +++ b/.gitignore @@ -69,4 +69,7 @@ id_rsa.pub record/ # For temperary development -develop_ws \ No newline at end of file +develop_ws + +# Keep nested standalone repos out of the RMCS index. +rmcs_ws/src/rmcs_dart_guidance/ diff --git a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml index efa82695..c441edd6 100644 --- a/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -6,15 +6,15 @@ rmcs_executor: - rmcs_dart_guidance::DartVisionCore -> dart_vision - rmcs_dart_guidance::ImagePublisher -> dart_camera_image_publisher - rmcs_dart_guidance::ImagePublisher -> dart_display_image_publisher - - rmcs_dart_guidance::manager::AutoAimOnlyComponent -> auto_aim_only - # - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager - # - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge - # - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge - # - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting - # - rmcs_core::controller::dart::DartFilling -> dart_filling + # - rmcs_dart_guidance::manager::AutoAimOnlyComponent -> auto_aim_only + - rmcs_dart_guidance::manager::DartManagerV2 -> dart_manager + - rmcs_dart_guidance::manager::RemoteCommandBridge -> remote_cmd_bridge + - rmcs_dart_guidance::manager::WebCommandBridge -> web_cmd_bridge + - rmcs_core::controller::dart::DartLaunchSettingV2 -> launch_setting + - rmcs_core::controller::dart::DartFilling -> dart_filling - rmcs_core::controller::dart::DartSettingController -> dart_setting - # - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller - # - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster + - rmcs_core::controller::pid::PidController -> force_screw_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> broadcaster dart_hardware: ros__parameters: @@ -27,11 +27,11 @@ dart_vision: invert_image: true exposure_time: 3000 gain: 11.0 - L_H: 0.0 + L_H: 45.0 L_S: 0.0 L_V: 200.0 - U_H: 180.0 - U_S: 30.0 + U_H: 90.0 + U_S: 255.0 U_V: 255.0 enable_image_saving: false image_save_directory: "./saved_images" @@ -92,7 +92,7 @@ dart_manager: aim_timeout_ticks: 5000 aim_timeout_returns_success: true aim_min_transform_rate: 1.0 - limiting_fill_ticks: 1000 + limiting_fill_ticks: 2000 lifting_stall_threshold: 0.1 # rad/s，低于此值视为堵转 lifting_stall_confirm_ticks: 100 # 连续帧数 = 0.1s @ 1kHz lifting_stall_min_run_ticks: 500 # 启动后最少运行帧数，避免启动瞬间误触发 @@ -113,8 +113,8 @@ launch_setting: dart_filling: ros__parameters: lifting_velocity: 3.0 - limiting_free_angle: 500 - limiting_lock_angle: 1000 + limiting_free_angle: 0x0BFF + limiting_lock_angle: 0x0B53 dart_setting: ros__parameters: diff --git a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp index ea7df39a..c038b189 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp @@ -26,13 +26,16 @@ class DartSettingController get_parameter("pitch_kd").as_double()} { register_input("/pitch/control/velocity", yaw_pitch_vel_setpoint_); - register_input("/dart/yaw_motor/velocity", yaw_velocity_); + register_input("/dart/yaw_motor/velocity", yaw_velocity_); register_input("/dart/pitch_motor/velocity", pitch_velocity_); register_input("/force/control/velocity", force_vel_setpoint_); - register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); + register_output("/dart/yaw_motor/control_torque", yaw_torque_, 0.0); register_output("/dart/pitch_motor/control_torque", pitch_torque_, 0.0); + register_input("/force_sensor/channel_1/weight", weight_ch1_); + register_input("/force_sensor/channel_2/weight", weight_ch2_); + try { log_enable_ = get_parameter("log_enable").as_bool(); } catch (...) { @@ -42,12 +45,12 @@ class DartSettingController void update() override { const Eigen::Vector2d& yaw_pitch_velocity = *yaw_pitch_vel_setpoint_; - *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); + *yaw_torque_ = yaw_pid_.update(yaw_pitch_velocity[0] - *yaw_velocity_); *pitch_torque_ = pitch_pid_.update(yaw_pitch_velocity[1] - *pitch_velocity_); if (log_enable_) { if (log_counter_++ >= 1000) { - RCLCPP_INFO(get_logger(), "Manual force control velocity: %.3f", *force_vel_setpoint_); + RCLCPP_INFO(get_logger(), "ch1 : %d | ch2 : %d", *weight_ch1_, *weight_ch2_); log_counter_ = 0; } } @@ -58,13 +61,16 @@ class DartSettingController pid::PidCalculator pitch_pid_; InputInterface yaw_pitch_vel_setpoint_; - InputInterface yaw_velocity_; - InputInterface pitch_velocity_; - InputInterface force_vel_setpoint_; + InputInterface yaw_velocity_; + InputInterface pitch_velocity_; + InputInterface force_vel_setpoint_; OutputInterface yaw_torque_; OutputInterface pitch_torque_; + InputInterface weight_ch1_; + InputInterface weight_ch2_; + bool log_enable_ = false; uint32_t log_counter_ = 0; }; @@ -72,5 +78,4 @@ class DartSettingController } // namespace rmcs_core::controller::dart #include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::dart::DartSettingController, rmcs_executor::Component) +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::dart::DartSettingController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 99d1122b..5df0bc84 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -52,8 +53,7 @@ class CatapultDartV3Lk , private librmcs::agent::CBoard { public: CatapultDartV3Lk() - : Node{get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , librmcs::agent::CBoard{get_parameter("serial_filter").as_string()} , dart_command_( create_partner_component(get_component_name() + "_command", *this)) @@ -91,11 +91,9 @@ class CatapultDartV3Lk .set_reduction_ratio(19.)); lifting_left_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10}.enable_multi_turn_angle()); lifting_right_motor_.configure( - device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10} - .enable_multi_turn_angle()); + device::LkMotor::Config{device::LkMotor::Type::kMG4005Ei10}.enable_multi_turn_angle()); first_sample_spot_ = this->get_parameter("first_sample_spot").as_double(); final_sample_spot_ = this->get_parameter("final_sample_spot").as_double(); @@ -107,15 +105,20 @@ class CatapultDartV3Lk register_output("/tf", tf_); register_output("/imu/catapult_pitch_angle", catapult_pitch_angle_); - register_output("/imu/catapult_roll_angle", catapult_roll_angle_); - register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); + register_output("/imu/catapult_roll_angle", catapult_roll_angle_); + register_output("/imu/catapult_yaw_angle", catapult_yaw_angle_); limiting_calibrate_subscription_ = create_subscription( - "/limiting/calibrate", rclcpp::QoS{0}, - [this](std_msgs::msg::Int32::UniquePtr&& msg) { + "/limiting/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { trigger_servo_calibrate_subscription_callback(limiting_servo_, std::move(msg)); }); + force_sensor_calibrate_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + force_sensor_calibrate_subscription_callback(std::move(msg)); + }); + register_output("/referee/serial", referee_serial_); referee_serial_->read = [this](std::byte* buffer, size_t size) -> size_t { std::lock_guard lock(referee_mutex_); @@ -128,16 +131,18 @@ class CatapultDartV3Lk }; referee_serial_->write = [this](const std::byte* buffer, size_t size) -> size_t { auto board = start_transmit(); - board.uart1_transmit({.uart_data = std::span{buffer, size}}); + board.uart1_transmit({ + .uart_data = std::span{buffer, size} + }); return size; }; } - CatapultDartV3Lk(const CatapultDartV3Lk&) = delete; + CatapultDartV3Lk(const CatapultDartV3Lk&) = delete; CatapultDartV3Lk& operator=(const CatapultDartV3Lk&) = delete; - CatapultDartV3Lk(CatapultDartV3Lk&&) = delete; - CatapultDartV3Lk& operator=(CatapultDartV3Lk&&) = delete; - ~CatapultDartV3Lk() override = default; + CatapultDartV3Lk(CatapultDartV3Lk&&) = delete; + CatapultDartV3Lk& operator=(CatapultDartV3Lk&&) = delete; + ~CatapultDartV3Lk() override = default; void update() override { dr16_.update_status(); @@ -162,7 +167,7 @@ class CatapultDartV3Lk // Force sensor: polling command on CAN1 (every 100 cycles) if (pub_time_count_++ > 100) { board.can1_transmit({ - .can_id = 0x301, + .can_id = 0x301, .can_data = device::CanPacket8{0}.as_bytes(), }); pub_time_count_ = 0; @@ -170,43 +175,47 @@ class CatapultDartV3Lk // DJI motors on CAN2 (pitch/yaw/force_screw: 0x200, belts: 0x1FF) board.can2_transmit({ - .can_id = 0x200, - .can_data = device::CanPacket8{ - pitch_motor_.generate_command(), - yaw_motor_.generate_command(), - force_screw_motor_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), + .can_id = 0x200, + .can_data = + device::CanPacket8{ + pitch_motor_.generate_command(), + yaw_motor_.generate_command(), + force_screw_motor_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), }); board.can2_transmit({ - .can_id = 0x1FF, - .can_data = device::CanPacket8{ - drive_belt_motor_left_.generate_command(), - drive_belt_motor_right_.generate_command(), - device::CanPacket8::PaddingQuarter{}, - device::CanPacket8::PaddingQuarter{}, - }.as_bytes(), + .can_id = 0x1FF, + .can_data = + device::CanPacket8{ + drive_belt_motor_left_.generate_command(), + drive_belt_motor_right_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), }); // LK4005 lifting motors on CAN1 (unicast per motor) - board.can1_transmit({ - .can_id = LK_LIFTING_LEFT_ID, - .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), - }); - board.can1_transmit({ - .can_id = LK_LIFTING_RIGHT_ID, - .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), - }); + // board.can1_transmit({ + // .can_id = LK_LIFTING_LEFT_ID, + // .can_data = lifting_left_motor_.generate_velocity_command().as_bytes(), + // }); + // board.can1_transmit({ + // .can_id = LK_LIFTING_RIGHT_ID, + // .can_data = lifting_right_motor_.generate_velocity_command().as_bytes(), + // }); // Limiting servo on UART2 (only send when target changes) if (!limiting_servo_.calibrate_mode()) { uint16_t current_target = limiting_servo_.get_target_angle(); if (current_target != last_limiting_angle_) { size_t uart_data_length; - auto command_buffer = - limiting_servo_.generate_runtime_command(uart_data_length); - board.uart2_transmit( - {.uart_data = std::span{command_buffer.get(), uart_data_length}}); + auto command_buffer = limiting_servo_.generate_runtime_command(uart_data_length); + board.uart2_transmit({ + .uart_data = std::span{command_buffer.get(), uart_data_length} + }); last_limiting_angle_ = current_target; } } @@ -222,8 +231,7 @@ class CatapultDartV3Lk force_sensor_frame_count_++; if (force_sensor_frame_count_ % 200 == 1) { RCLCPP_INFO( - logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s", - data.can_data.size(), + logger_, "[ForceSensor CAN1:0x302] raw(%zu): %s", data.can_data.size(), bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); } force_sensor_.store_status(data.can_data); @@ -239,13 +247,12 @@ class CatapultDartV3Lk return; const auto can_id = data.can_id; - if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 - && can_id != 0x205 && can_id != 0x206) { + if (can_id != 0x201 && can_id != 0x202 && can_id != 0x203 && can_id != 0x205 + && can_id != 0x206) { can2_unknown_count_++; if (can2_unknown_count_ % 50 == 1) { RCLCPP_INFO( - logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", - can_id, data.can_data.size(), + logger_, "[CAN2 unknown] id=0x%03X raw(%zu): %s", can_id, data.can_data.size(), bytes_to_hex_string(data.can_data.data(), data.can_data.size()).c_str()); } } @@ -272,10 +279,11 @@ class CatapultDartV3Lk void uart2_receive_callback(const librmcs::data::UartDataView& data) override { const auto* raw_data = data.uart_data.data(); - const size_t length = data.uart_data.size(); + const size_t length = data.uart_data.size(); - RCLCPP_DEBUG(logger_, "UART2 received: len=%zu [%s]", length, - bytes_to_hex_string(raw_data, length).c_str()); + RCLCPP_DEBUG( + logger_, "UART2 received: len=%zu [%s]", length, + bytes_to_hex_string(raw_data, length).c_str()); if (length < 3) { RCLCPP_WARN(logger_, "UART2 data too short: %zu bytes", length); @@ -298,8 +306,7 @@ class CatapultDartV3Lk dr16_.store_status(data.uart_data.data(), data.uart_data.size()); } - void accelerometer_receive_callback( - const librmcs::data::AccelerometerDataView& data) override { + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { imu_.store_accelerometer_status(data.x, data.y, data.z); } @@ -308,10 +315,18 @@ class CatapultDartV3Lk } private: - static constexpr uint8_t LIMITING_SERVO_ID = 0x03; - static constexpr uint32_t LK_LIFTING_LEFT_ID = 0x141; + static constexpr uint8_t LIMITING_SERVO_ID = 0x03; + static constexpr uint32_t LK_LIFTING_LEFT_ID = 0x141; static constexpr uint32_t LK_LIFTING_RIGHT_ID = 0x145; + void force_sensor_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr msg) { + auto board = start_transmit(); + board.can1_transmit({ + .can_id = 0x201, + .can_data = device::CanPacket8{0}.as_bytes(), + }); + } + void trigger_servo_calibrate_subscription_callback( device::TriggerServo& servo, std_msgs::msg::Int32::UniquePtr msg) { servo.set_calibrate_mode(msg->data); @@ -344,10 +359,12 @@ class CatapultDartV3Lk if (command_buffer && command_length > 0) { auto board = start_transmit(); - board.uart2_transmit( - {.uart_data = std::span{command_buffer.get(), command_length}}); - RCLCPP_INFO(logger_, "UART2 Pub: (length=%zu)[ %s ]", command_length, - bytes_to_hex_string(command_buffer.get(), command_length).c_str()); + board.uart2_transmit({ + .uart_data = std::span{command_buffer.get(), command_length} + }); + RCLCPP_INFO( + logger_, "UART2 Pub: (length=%zu)[ %s ]", command_length, + bytes_to_hex_string(command_buffer.get(), command_length).c_str()); } } @@ -373,19 +390,18 @@ class CatapultDartV3Lk void processImuData() { auto current_time = std::chrono::steady_clock::now(); - double elapsed_seconds = - std::chrono::duration(current_time - start_time_).count(); + double elapsed_seconds = std::chrono::duration(current_time - start_time_).count(); Eigen::Quaterniond imu_quat(imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()); Eigen::Matrix3d rot = imu_quat.toRotationMatrix(); - double roll = std::atan2(rot(2, 1), rot(2, 2)); + double roll = std::atan2(rot(2, 1), rot(2, 2)); double pitch = std::asin(-rot(2, 0)); - double yaw = std::atan2(rot(1, 0), rot(0, 0)); + double yaw = std::atan2(rot(1, 0), rot(0, 0)); - double t_roll = -roll_filter_.update(roll); - double t_pitch = pitch_filter_.update(pitch); - double t_yaw = -yaw_filter_.update(yaw); + double t_roll = -roll_filter_.update(roll); + double t_pitch = pitch_filter_.update(pitch); + double t_yaw = -yaw_filter_.update(yaw); if (elapsed_seconds >= first_sample_spot_ && elapsed_seconds <= final_sample_spot_) { yaw_samples_.push_back(t_yaw); @@ -394,8 +410,8 @@ class CatapultDartV3Lk double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0; double n = static_cast(yaw_samples_.size()); for (size_t i = 0; i < yaw_samples_.size(); ++i) { - sum_x += time_samples_[i]; - sum_y += yaw_samples_[i]; + sum_x += time_samples_[i]; + sum_y += yaw_samples_[i]; sum_xy += time_samples_[i] * yaw_samples_[i]; sum_xx += time_samples_[i] * time_samples_[i]; } @@ -407,50 +423,53 @@ class CatapultDartV3Lk t_yaw -= (yaw_drift_coefficient_ + 0.000512) * elapsed_seconds; publishTfTransforms(t_roll, t_pitch, t_yaw); - *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; + *catapult_roll_angle_ = (t_roll / M_PI) * 180.0; *catapult_pitch_angle_ = (t_pitch / M_PI) * 180.0; - *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; + *catapult_yaw_angle_ = (t_yaw / M_PI) * 180.0; } void publishTfTransforms(double roll, double pitch, double yaw) { auto now = this->get_clock()->now(); - auto make_tf = - [&](const std::string& parent, const std::string& child, - const geometry_msgs::msg::Vector3& trans, - const geometry_msgs::msg::Quaternion& rot) { - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = now; - tf.header.frame_id = parent; - tf.child_frame_id = child; - tf.transform.translation = trans; - tf.transform.rotation = rot; - return tf; - }; + auto make_tf = [&](const std::string& parent, const std::string& child, + const geometry_msgs::msg::Vector3& trans, + const geometry_msgs::msg::Quaternion& rot) { + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = now; + tf.header.frame_id = parent; + tf.child_frame_id = child; + tf.transform.translation = trans; + tf.transform.rotation = rot; + return tf; + }; auto make_vec = [](double x, double y, double z) { geometry_msgs::msg::Vector3 v; - v.x = x; v.y = y; v.z = z; + v.x = x; + v.y = y; + v.z = z; return v; }; auto make_quat = [](double x, double y, double z, double w) { geometry_msgs::msg::Quaternion q; - q.x = x; q.y = y; q.z = z; q.w = w; + q.x = x; + q.y = y; + q.z = z; + q.w = w; return q; }; - Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); + Eigen::Quaterniond roll_q(Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())); Eigen::Quaterniond pitch_q(Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())); - Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); + Eigen::Quaterniond yaw_q(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); Eigen::Quaterniond combined = yaw_q * pitch_q * roll_q; - auto zero = make_vec(0, 0, 0); + auto zero = make_vec(0, 0, 0); auto p_trans = make_vec(0, 0, 0.05); - auto id_rot = make_quat(0, 0, 0, 1); + auto id_rot = make_quat(0, 0, 0, 1); - tf_broadcaster_->sendTransform( - make_tf("base_link", "gimbal_center_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf("base_link", "gimbal_center_link", zero, id_rot)); tf_broadcaster_->sendTransform(make_tf( "gimbal_center_link", "yaw_link", zero, make_quat(yaw_q.x(), yaw_q.y(), yaw_q.z(), yaw_q.w()))); @@ -460,8 +479,7 @@ class CatapultDartV3Lk tf_broadcaster_->sendTransform(make_tf( "pitch_link", "odom_imu", zero, make_quat(combined.x(), combined.y(), combined.z(), combined.w()))); - tf_broadcaster_->sendTransform( - make_tf("world", "base_link", zero, id_rot)); + tf_broadcaster_->sendTransform(make_tf("world", "base_link", zero, id_rot)); } class DartCommand : public rmcs_executor::Component { @@ -494,14 +512,14 @@ class CatapultDartV3Lk device::ForceSensor force_sensor_; - device::PWMServo trigger_servo_; + device::PWMServo trigger_servo_; device::TriggerServo limiting_servo_; // LK4005 lifting motors (replaces TriggerServo lifting servos) device::LkMotor lifting_left_motor_; device::LkMotor lifting_right_motor_; - device::Dr16 dr16_; + device::Dr16 dr16_; device::Bmi088 imu_; std::unique_ptr tf_broadcaster_; @@ -512,17 +530,18 @@ class CatapultDartV3Lk OutputInterface catapult_yaw_angle_; rclcpp::Subscription::SharedPtr limiting_calibrate_subscription_; + rclcpp::Subscription::SharedPtr force_sensor_calibrate_; std::mutex referee_mutex_; std::deque referee_ring_buffer_receive_; OutputInterface referee_serial_; - int pub_time_count_ = 0; + int pub_time_count_ = 0; int force_sensor_frame_count_ = 0; - int can2_unknown_count_ = 0; + int can2_unknown_count_ = 0; - double first_sample_spot_ = 1.0; - double final_sample_spot_ = 4.0; + double first_sample_spot_ = 1.0; + double final_sample_spot_ = 4.0; double yaw_drift_coefficient_ = 0.0; std::vector yaw_samples_; std::vector time_samples_; diff --git a/rmcs_ws/src/rmcs_dart_guidance b/rmcs_ws/src/rmcs_dart_guidance deleted file mode 160000 index 26f3e1f9..00000000 --- a/rmcs_ws/src/rmcs_dart_guidance +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 26f3e1f942dc05a1db7f3d2830f4d4865c32d3ce From b32509df94ad13d08aa789c8e1ea8faf2178fea0 Mon Sep 17 00:00:00 2001 From: floatpigeon Date: Wed, 18 Mar 2026 22:08:27 +0800 Subject: [PATCH 45/45] Adjust dart force sensor calibration --- rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp index 5df0bc84..9a8d475a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -114,7 +114,7 @@ class CatapultDartV3Lk }); force_sensor_calibrate_ = create_subscription( - "/force_sensor/calibrate", rclcpp::QoS{0}, + "/force_sensor/calibrate", rclcpp::QoS{10}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { force_sensor_calibrate_subscription_callback(std::move(msg)); }); @@ -323,8 +323,9 @@ class CatapultDartV3Lk auto board = start_transmit(); board.can1_transmit({ .can_id = 0x201, - .can_data = device::CanPacket8{0}.as_bytes(), + .can_data = device::CanPacket8{0x0F}.as_bytes(), }); + RCLCPP_INFO(logger_, "calibrate"); } void trigger_servo_calibrate_subscription_callback(