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/.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/.github/workflows/update-image.yml b/.github/workflows/update-image.yml new file mode 100644 index 00000000..71c5538f --- /dev/null +++ b/.github/workflows/update-image.yml @@ -0,0 +1,49 @@ +name: Build and Push RMCS Images + +on: + workflow_dispatch: + push: + paths: + - 'Dockerfile' + branches: + - main + +jobs: + build-and-push: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Set up SSH keys + run: | + echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa + echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub + chmod 600 .ssh/id_rsa + chmod 644 .ssh/id_rsa.pub + + - name: Build and push rmcs-develop:latest + uses: docker/build-push-action@v6 + with: + context: . + push: true + target: rmcs-develop + tags: qzhhhi/rmcs-develop:latest + + - name: Build and push rmcs-runtime:latest + uses: docker/build-push-action@v6 + with: + context: . + push: true + target: rmcs-runtime + tags: qzhhhi/rmcs-runtime:latest diff --git a/.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/.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..ef5d85ff 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 +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/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 new file mode 100755 index 00000000..8f367049 --- /dev/null +++ b/.script/play-autoaim @@ -0,0 +1,69 @@ +#!/bin/sh + +set -e + +MONITOR_USER="" +MONITOR_PLAYER="vlc" +MONITOR_HOST="localhost" + +SDP_PATH="/tmp/auto_aim.sdp" + +USE_REMOTE=false +SKIP_COPY=false + +# 参数解析 +while [ "$#" -gt 0 ]; do + case "$1" in + --user) + shift + [ -z "$1" ] && echo "❌ --user 参数缺失" && exit 1 + MONITOR_USER="$1" + ;; + --remote) + USE_REMOTE=true + ;; + --no-copy) + SKIP_COPY=true + ;; + *) + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" + exit 1 + ;; + esac + shift +done + +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" + exit 1 +fi + +if [ "$USE_REMOTE" = true ]; then + scp remote:${SDP_PATH} ${SDP_PATH} + if [ $? -ne 0 ]; then + echo " 从 remote 拷贝失败。是否继续？(y/n)" + read -r answer + case "$answer" in + [Yy]*) echo " 继续执行后续操作…" ;; + *) + 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 "${MONITOR_USER}@${MONITOR_HOST}" "DISPLAY=:0 ${MONITOR_PLAYER} '${SDP_PATH}' --network-caching=50" diff --git a/.script/sync-remote b/.script/sync-remote index 60a2b816..7d62782b 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 = os.getenv("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..f8ab1dd2 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,13 +1,15 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RCUTILS_COLORIZED_OUTPUT=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..2f7a0ee6 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,16 +1,22 @@ #!/bin/bash +export RCUTILS_COLORIZED_OUTPUT=1 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)" eval "$(register-python-argcomplete colcon)" export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit 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/Dockerfile b/Dockerfile index 37bf1097..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 \ @@ -123,7 +124,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 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} 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..c441edd6 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/dart-guidance.yaml @@ -0,0 +1,142 @@ +rmcs_executor: + ros__parameters: + 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::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 + +dart_hardware: + ros__parameters: + serial_filter: "" + 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: 45.0 + L_S: 0.0 + L_V: 200.0 + U_H: 90.0 + U_S: 255.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 + +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: 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.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: 2000 + 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 + +launch_setting: + ros__parameters: + 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: 1.0 + b_ki: 0.0 + b_kd: 0.0 + belt_hold_torque: 15.0 # manager 未提供保留力矩时的回退值 + +dart_filling: + ros__parameters: + lifting_velocity: 3.0 + limiting_free_angle: 0x0BFF + limiting_lock_angle: 0x0B53 + +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 + log_enable: true + +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 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/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 deleted file mode 160000 index b82f2eaf..00000000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368..51ba3ca0 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -98,4 +98,67 @@ Test plugin. - \ No newline at end of file + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + Test plugin. + + + 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. + + + Launch setting V2: belt + trigger execution with task-level velocity, torque limit, and hold torque. + + + 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. + + 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..c038b189 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/dart_setting.cpp @@ -0,0 +1,81 @@ +#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_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); + + 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 (...) { + 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(), "ch1 : %d | ch2 : %d", *weight_ch1_, *weight_ch2_); + log_counter_ = 0; + } + } + } + +private: + pid::PidCalculator yaw_pid_; + pid::PidCalculator pitch_pid_; + + InputInterface yaw_pitch_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; +}; + +} // 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/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 new file mode 100644 index 00000000..3faedc4d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/dart/launch_setting.cpp @@ -0,0 +1,163 @@ +#include +#include + +#include +#include +#include +#include + +#include "controller/pid/matrix_pid_calculator.hpp" + +namespace rmcs_core::controller::dart { + +// DartLaunchSettingV2 +// 同步带与扳机执行组件： +// - 速度、限扭、保留力矩由上层 DartManagerV2 下发 +// - 本组件仅负责 belt PID 同步控制和扳机量值映射 +// - 升降机构与限位舵机由 DartFilling 独立处理 +class DartLaunchSettingV2 + : public rmcs_executor::Component + , public rclcpp::Node { +public: + DartLaunchSettingV2() + : Node{get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , 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(); + 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_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 { + 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_mode = BeltControlMode::MOVE_DOWN; + control_velocity = +requested_velocity; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::DOWN; + break; + case rmcs_msgs::DartSliderStatus::UP: + control_mode = BeltControlMode::MOVE_UP; + control_velocity = -requested_velocity; + prev_belt_cmd_ = rmcs_msgs::DartSliderStatus::UP; + break; + default: + control_mode = + belt_wait_zero_velocity_.ready() && *belt_wait_zero_velocity_ + ? BeltControlMode::WAIT_ZERO + : BeltControlMode::WAIT_HOLD; + break; + } + + if (control_mode != control_mode_) { + belt_pid_.reset(); + control_mode_ = control_mode; + } + + double torque_limit = + belt_torque_limit_.ready() ? std::abs(*belt_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: + 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_}; + 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); + } + + pid::MatrixPidCalculator<2> belt_pid_; + + double belt_velocity_; + double sync_coefficient_; + double max_control_torque_; + double trigger_free_value_; + double trigger_lock_value_; + 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_; + + OutputInterface left_belt_torque_; + OutputInterface right_belt_torque_; + OutputInterface trigger_value_; +}; + +} // 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.cpp b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp new file mode 100644 index 00000000..9a8d475a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/catapult_dart.cpp @@ -0,0 +1,556 @@ +#include +#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 左, 0x145 右)。 +限位舵机保留 TriggerServo (UART2, ID=0x03)。 + +升降电机接口 (double, rad): + 输出: /dart/lifting_left/angle, /dart/lifting_left/velocity 等 + 输入: /dart/lifting_left/control_velocity (由 DartFilling 写入) +*/ + +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)); + }); + + force_sensor_calibrate_ = create_subscription( + "/force_sensor/calibrate", rclcpp::QoS{10}, + [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_); + 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{0}.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 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{0x0F}.as_bytes(), + }); + RCLCPP_INFO(logger_, "calibrate"); + } + + 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_; + 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 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/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/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 new file mode 100644 index 00000000..9a4e991d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/force_sensor.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include "hardware/device/can_packet.hpp" +#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) { + 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() { + 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; } + 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, ch1_1, ch1_2, ch1_3; + uint8_t ch2_0, ch2_1, ch2_2, 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/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_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 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..f75e92b7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/trigger_servo.hpp @@ -0,0 +1,386 @@ +#pragma once + +#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, + uint8_t id) + : id_(id) { + 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; } + uint16_t get_target_angle() const { + return *control_angle_; + } + + 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 = 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; + return buffer; + } + + 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; + } + + 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, 0x0000}; + } + + 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, 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, 0x0000}; + } + + 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: + 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)) 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; + 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; + } + }; + + 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/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 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_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) { 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..bebe7a91 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/dart_launch_stage.hpp @@ -0,0 +1,37 @@ +namespace rmcs_msgs { +enum class DartLaunchStages { + DISABLE, + INIT, + LOADING, + RESETTING, + READY, + CANCEL, +}; +/* +说明： + DISABLE：电机全部失能，遥控器双下 + INIT：初始状态，需要滑台复位 + LOADING：上膛和填装中 + RESETTING: 同步带复位中 + READY：可发射状态。发射完成后进入INIT +特殊： + 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 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 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 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 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