├── .clang-format ├── .clang-tidy ├── .devcontainer └── devcontainer.json ├── .gitignore ├── .gitmodules ├── .script ├── attach-remote ├── build-rmcs ├── clean-rmcs ├── launch-rmcs ├── set-remote ├── set-robot ├── ssh-remote ├── sync-remote ├── template │ ├── entrypoint │ ├── env_setup.bash │ ├── env_setup.zsh │ ├── rmcs-service │ └── set-hostname └── wait-sync ├── .ssh └── config ├── .vscode └── settings.default.json ├── Dockerfile ├── README.md ├── docs └── zh-cn │ ├── build_docker_image.md │ ├── docker_with_proxy.md │ ├── fix_devcontainer_stuck.md │ └── wsl2_develop_guide.md └── rmcs_ws └── src ├── rmcs_bringup ├── CMakeLists.txt ├── LICENSE ├── config │ ├── auto_aim_test.yaml │ ├── hero.yaml │ ├── infantry_a.yaml │ ├── infantry_b.yaml │ └── sentry.yaml ├── launch │ └── rmcs.launch.py └── package.xml ├── rmcs_core ├── CMakeLists.txt ├── include │ └── rmcs_core │ │ └── .gitkeep ├── package.xml ├── plugins.xml └── src │ ├── broadcaster │ ├── tf_broadcaster.cpp │ └── value_broadcaster.cpp │ ├── controller │ ├── chassis │ │ ├── chassis_controller.cpp │ │ └── omni_wheel_controller.cpp │ ├── gimbal │ │ ├── gimbal_controller.cpp │ │ ├── shooting_controller.cpp │ │ └── shooting_recorder.cpp │ └── pid │ │ ├── error_pid_controller.cpp │ │ ├── matrix_pid_calculator.hpp │ │ ├── pid_calculator.hpp │ │ └── pid_controller.cpp │ ├── hardware │ ├── device │ │ ├── benewake.hpp │ │ ├── bmi088.hpp │ │ ├── dji_motor.hpp │ │ ├── dr16.hpp │ │ ├── gy614.hpp │ │ ├── hipnuc.hpp │ │ ├── lk_motor.hpp │ │ └── supercap.hpp │ ├── hero.cpp │ ├── infantry.cpp │ └── tunnel_infantry.cpp │ └── referee │ ├── app │ └── ui │ │ ├── hero.cpp │ │ ├── infantry.cpp │ │ ├── shape │ │ ├── cfs_scheduler.hpp │ │ ├── red_black_tree.hpp │ │ ├── remote_shape.hpp │ │ └── shape.hpp │ │ └── widget │ │ ├── crosshair.hpp │ │ └── status_ring.hpp │ ├── command.cpp │ ├── command │ ├── field.hpp │ ├── interaction.cpp │ ├── interaction │ │ ├── communicate.cpp │ │ ├── header.hpp │ │ ├── sentry_decision.cpp │ │ └── ui.cpp │ ├── map_marker.cpp │ └── text_display.cpp │ ├── frame.hpp │ ├── status.cpp │ └── status │ └── field.hpp ├── rmcs_description ├── CMakeLists.txt ├── LICENSE ├── config │ └── .gitkeep ├── include │ └── rmcs_description │ │ └── tf_description.hpp ├── launch │ └── display.launch.py ├── meshes │ ├── collision │ │ └── .gitkeep │ └── visual │ │ ├── base_link.stl │ │ ├── pitch_link.stl │ │ ├── wheel_link.stl │ │ └── yaw_link.stl ├── package.xml ├── rviz │ └── display.rviz ├── src │ └── tf_description.cpp └── urdf │ └── medium_feed_omni_wheel.xacro ├── rmcs_executor ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── rmcs_executor │ │ └── component.hpp ├── package.xml └── src │ ├── component.cpp │ ├── executor.hpp │ ├── main.cpp │ └── predefined_msg_provider.hpp ├── rmcs_msgs ├── CMakeLists.txt ├── include │ └── rmcs_msgs │ │ ├── chassis_mode.hpp │ │ ├── full_robot_id.hpp │ │ ├── game_stage.hpp │ │ ├── keyboard.hpp │ │ ├── mouse.hpp │ │ ├── robot_color.hpp │ │ ├── robot_id.hpp │ │ ├── serial_interface.hpp │ │ ├── shoot_mode.hpp │ │ ├── shoot_status.hpp │ │ └── switch.hpp ├── msg │ ├── ArmorPlate.msg │ ├── ArmorPlateArray.msg │ ├── GameStatus.msg │ ├── Point2f.msg │ ├── RobotPose.msg │ └── RobotStatus.msg └── package.xml └── rmcs_utility ├── CMakeLists.txt ├── LICENSE ├── include └── rmcs_utility │ ├── crc │ └── dji_crc.hpp │ ├── double_buffer.hpp │ ├── fps_counter.hpp │ ├── package_receive.hpp │ ├── ring_buffer.hpp │ └── tick_timer.hpp └── package.xml /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: LLVM 2 | 3 | ColumnLimit: 100 4 | 5 | IndentWidth: 4 6 | ContinuationIndentWidth: 4 7 | ConstructorInitializerIndentWidth: 4 8 | AccessModifierOffset: -4 9 | 10 | Standard: c++20 11 | 12 | MaxEmptyLinesToKeep: 1 13 | 14 | AlignAfterOpenBracket: AlwaysBreak 15 | 16 | AlignArrayOfStructures: Right 17 | 18 | AlignConsecutiveAssignments: 19 | Enabled: true 20 | AcrossEmptyLines: false 21 | AcrossComments: false 22 | AlignCompound: false 23 | PadOperators: false 24 | AlignConsecutiveBitFields: 25 | Enabled: true 26 | AcrossEmptyLines: false 27 | AcrossComments: false 28 | AlignConsecutiveDeclarations: 29 | Enabled: false 30 | AlignConsecutiveMacros: 31 | Enabled: true 32 | AcrossEmptyLines: false 33 | AcrossComments: false 34 | # AlignConsecutiveShortCaseStatements: 35 | # Enabled: false 36 | AlignEscapedNewlines: Left 37 | AlignOperands: AlignAfterOperator 38 | AlignTrailingComments: 39 | Kind: Always 40 | OverEmptyLines: 64 41 | PointerAlignment: Left 42 | 43 | AllowAllArgumentsOnNextLine: true 44 | AllowAllParametersOfDeclarationOnNextLine: true 45 | AllowShortBlocksOnASingleLine: Empty 46 | AllowShortCaseLabelsOnASingleLine: true 47 | 48 | IndentPPDirectives: AfterHash 49 | PPIndentWidth: 1 50 | 51 | BreakBeforeBinaryOperators: NonAssignment 52 | BreakBeforeConceptDeclarations: Always 53 | BreakConstructorInitializers: BeforeComma 54 | BreakInheritanceList: BeforeComma 55 | FixNamespaceComments: true 56 | IncludeBlocks: Preserve 57 | IndentWrappedFunctionNames: true 58 | 59 | AlwaysBreakTemplateDeclarations: Yes 60 | IndentRequiresClause: false 61 | RequiresClausePosition: SingleLine 62 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | # Generated from CLion Inspection settings 2 | --- 3 | Checks: '-*, 4 | bugprone-argument-comment, 5 | bugprone-assert-side-effect, 6 | bugprone-bad-signal-to-kill-thread, 7 | bugprone-branch-clone, 8 | bugprone-copy-constructor-init, 9 | bugprone-dangling-handle, 10 | bugprone-dynamic-static-initializers, 11 | bugprone-fold-init-type, 12 | bugprone-forward-declaration-namespace, 13 | bugprone-forwarding-reference-overload, 14 | bugprone-inaccurate-erase, 15 | bugprone-incorrect-roundings, 16 | bugprone-integer-division, 17 | bugprone-lambda-function-name, 18 | bugprone-macro-parentheses, 19 | bugprone-macro-repeated-side-effects, 20 | bugprone-misplaced-operator-in-strlen-in-alloc, 21 | bugprone-misplaced-pointer-arithmetic-in-alloc, 22 | bugprone-misplaced-widening-cast, 23 | bugprone-move-forwarding-reference, 24 | bugprone-multiple-statement-macro, 25 | bugprone-no-escape, 26 | bugprone-not-null-terminated-result, 27 | bugprone-parent-virtual-call, 28 | bugprone-posix-return, 29 | bugprone-reserved-identifier, 30 | bugprone-sizeof-container, 31 | bugprone-sizeof-expression, 32 | bugprone-spuriously-wake-up-functions, 33 | bugprone-string-constructor, 34 | bugprone-string-integer-assignment, 35 | bugprone-string-literal-with-embedded-nul, 36 | bugprone-suspicious-enum-usage, 37 | bugprone-suspicious-include, 38 | bugprone-suspicious-memset-usage, 39 | bugprone-suspicious-missing-comma, 40 | bugprone-suspicious-semicolon, 41 | bugprone-suspicious-string-compare, 42 | bugprone-suspicious-memory-comparison, 43 | bugprone-suspicious-realloc-usage, 44 | bugprone-swapped-arguments, 45 | bugprone-terminating-continue, 46 | bugprone-throw-keyword-missing, 47 | bugprone-too-small-loop-variable, 48 | bugprone-undefined-memory-manipulation, 49 | bugprone-undelegated-constructor, 50 | bugprone-unhandled-self-assignment, 51 | bugprone-unused-raii, 52 | bugprone-unused-return-value, 53 | bugprone-use-after-move, 54 | bugprone-virtual-near-miss, 55 | cert-dcl21-cpp, 56 | cert-dcl58-cpp, 57 | cert-err34-c, 58 | cert-err52-cpp, 59 | cert-err60-cpp, 60 | cert-flp30-c, 61 | cert-msc50-cpp, 62 | cert-msc51-cpp, 63 | cert-str34-c, 64 | cppcoreguidelines-interfaces-global-init, 65 | cppcoreguidelines-narrowing-conversions, 66 | # cppcoreguidelines-pro-type-member-init, 67 | cppcoreguidelines-pro-type-static-cast-downcast, 68 | cppcoreguidelines-slicing, 69 | google-default-arguments, 70 | google-explicit-constructor, 71 | google-runtime-operator, 72 | hicpp-exception-baseclass, 73 | hicpp-multiway-paths-covered, 74 | misc-misplaced-const, 75 | misc-new-delete-overloads, 76 | # misc-no-recursion, 77 | misc-non-copyable-objects, 78 | misc-throw-by-value-catch-by-reference, 79 | misc-unconventional-assign-operator, 80 | misc-uniqueptr-reset-release, 81 | modernize-avoid-bind, 82 | modernize-concat-nested-namespaces, 83 | modernize-deprecated-headers, 84 | modernize-deprecated-ios-base-aliases, 85 | modernize-loop-convert, 86 | modernize-make-shared, 87 | modernize-make-unique, 88 | modernize-pass-by-value, 89 | modernize-raw-string-literal, 90 | modernize-redundant-void-arg, 91 | modernize-replace-auto-ptr, 92 | modernize-replace-disallow-copy-and-assign-macro, 93 | modernize-replace-random-shuffle, 94 | modernize-return-braced-init-list, 95 | modernize-shrink-to-fit, 96 | modernize-unary-static-assert, 97 | # modernize-use-auto, 98 | modernize-use-bool-literals, 99 | modernize-use-emplace, 100 | modernize-use-equals-default, 101 | modernize-use-equals-delete, 102 | # modernize-use-nodiscard 103 | modernize-use-noexcept, 104 | modernize-use-nullptr, 105 | modernize-use-override, 106 | modernize-use-transparent-functors, 107 | modernize-use-uncaught-exceptions, 108 | mpi-buffer-deref, 109 | mpi-type-mismatch, 110 | openmp-use-default-none, 111 | performance-faster-string-find, 112 | performance-for-range-copy, 113 | performance-implicit-conversion-in-loop, 114 | performance-inefficient-algorithm, 115 | performance-inefficient-string-concatenation, 116 | performance-inefficient-vector-operation, 117 | performance-move-const-arg, 118 | performance-move-constructor-init, 119 | performance-no-automatic-move, 120 | performance-noexcept-move-constructor, 121 | performance-trivially-destructible, 122 | performance-type-promotion-in-math-fn, 123 | performance-unnecessary-copy-initialization, 124 | performance-unnecessary-value-param, 125 | portability-simd-intrinsics, 126 | readability-avoid-const-params-in-decls, 127 | readability-const-return-type, 128 | readability-container-size-empty, 129 | readability-convert-member-functions-to-static, 130 | readability-delete-null-pointer, 131 | readability-deleted-default, 132 | readability-inconsistent-declaration-parameter-name, 133 | readability-make-member-function-const, 134 | readability-misleading-indentation, 135 | readability-misplaced-array-index, 136 | readability-non-const-parameter, 137 | readability-redundant-control-flow, 138 | readability-redundant-declaration, 139 | readability-redundant-function-ptr-dereference, 140 | readability-redundant-smartptr-get, 141 | readability-redundant-string-cstr, 142 | readability-redundant-string-init, 143 | readability-simplify-subscript-expr, 144 | # readability-static-accessed-through-instance, 145 | readability-static-definition-in-anonymous-namespace, 146 | readability-string-compare, 147 | readability-uniqueptr-delete-release, 148 | readability-use-anyofallof' -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | // For format details, see https://aka.ms/devcontainer.json. For config options, see the 2 | // README at: https://github.com/devcontainers/templates/tree/main/src/docker-existing-dockerfile 3 | { 4 | "name": "rmcs-develop", 5 | "image": "qzhhhi/rmcs-develop:latest", 6 | "privileged": true, 7 | "mounts": [ 8 | { 9 | "source": "/dev", 10 | "target": "/dev", 11 | "type": "bind" 12 | }, 13 | { 14 | "source": "/tmp/.X11-unix", 15 | "target": "/tmp/.X11-unix", 16 | "type": "bind" 17 | } 18 | ], 19 | "containerEnv": { 20 | "DISPLAY": "${localEnv:DISPLAY}" 21 | }, 22 | "runArgs": [ 23 | "--network", 24 | "host" 25 | ], 26 | "customizations": { 27 | "vscode": { 28 | "extensions": [ 29 | // C++ language support 30 | "llvm-vs-code-extensions.vscode-clangd", 31 | // Python language support 32 | "ms-python.vscode-pylance", 33 | "ms-python.python", 34 | "ms-python.debugpy", 35 | // CMake language support 36 | "twxs.cmake", 37 | // Code spell checking 38 | "streetsidesoftware.code-spell-checker", 39 | // Git enhancements 40 | "mhutchie.git-graph" 41 | ] 42 | } 43 | } 44 | } -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | log/ 3 | build/ 4 | install/ 5 | bin/ 6 | lib/ 7 | msg_gen/ 8 | srv_gen/ 9 | msg/*Action.msg 10 | msg/*ActionFeedback.msg 11 | msg/*ActionGoal.msg 12 | msg/*ActionResult.msg 13 | msg/*Feedback.msg 14 | msg/*Goal.msg 15 | msg/*Result.msg 16 | msg/_*.py 17 | build_isolated/ 18 | devel_isolated/ 19 | 20 | # Generated by dynamic reconfigure 21 | *.cfgc 22 | /cfg/cpp/ 23 | /cfg/*.py 24 | 25 | # Ignore generated docs 26 | *.dox 27 | *.wikidoc 28 | 29 | # eclipse stuff 30 | .project 31 | .cproject 32 | 33 | # qcreator stuff 34 | CMakeLists.txt.user 35 | 36 | srv/_*.py 37 | *.pcd 38 | *.pyc 39 | qtcreator-* 40 | *.user 41 | 42 | /planning/cfg 43 | /planning/docs 44 | /planning/src 45 | 46 | *~ 47 | 48 | # Emacs 49 | .#* 50 | 51 | # Catkin custom files 52 | CATKIN_IGNORE 53 | 54 | # Clangd cache files 55 | .cache/ 56 | 57 | # VSCode configure files 58 | .vscode/ 59 | 60 | # Hik camera log files 61 | MvFGSdkLog 62 | MvSdkLog 63 | 64 | # SSH key files 65 | id_rsa 66 | id_rsa.pub 67 | 68 | # Recording files 69 | record/ 70 | 71 | # For temperary development 72 | develop_ws -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "rmcs_ws/src/rmcs_core/librmcs"] 2 | path = rmcs_ws/src/rmcs_core/librmcs 3 | url = https://github.com/Alliance-Algorithm/librmcs.git 4 | [submodule "rmcs_ws/src/fast_tf"] 5 | path = rmcs_ws/src/fast_tf 6 | url = https://github.com/qzhhhi/FastTF.git 7 | [submodule "rmcs_ws/src/rmcs_auto_aim"] 8 | path = rmcs_ws/src/rmcs_auto_aim 9 | url = https://github.com/Alliance-Algorithm/rmcs_auto_aim.git 10 | [submodule "rmcs_ws/src/hikcamera"] 11 | path = rmcs_ws/src/hikcamera 12 | url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git 13 | [submodule "rmcs_ws/src/serial"] 14 | path = rmcs_ws/src/serial 15 | url = git@github.com:Alliance-Algorithm/ros2-serial.git 16 | -------------------------------------------------------------------------------- /.script/attach-remote: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # if arg[1] == "-r" 4 | if [ "$1" == "-r" ]; then 5 | ssh-remote "service rmcs restart && service rmcs attach" 6 | else 7 | ssh-remote "service rmcs attach" 8 | fi 9 | -------------------------------------------------------------------------------- /.script/build-rmcs: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | source /opt/ros/humble/setup.bash 4 | 5 | cd /workspaces/RMCS/rmcs_ws 6 | 7 | colcon build --symlink-install --merge-install 8 | -------------------------------------------------------------------------------- /.script/clean-rmcs: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | rm -rf /workspaces/RMCS/rmcs_ws/build /workspaces/RMCS/rmcs_ws/install /workspaces/RMCS/rmcs_ws/log -------------------------------------------------------------------------------- /.script/launch-rmcs: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | source ~/env_setup.bash 4 | 5 | if [ -z "${RMCS_ROBOT_TYPE}" ]; then 6 | echo "Error: Please set robot type first (e.g. 'set-robot sentry')." 7 | exit 1 8 | fi 9 | 10 | ros2 launch rmcs_bringup rmcs.launch.py robot:=${RMCS_ROBOT_TYPE} 11 | -------------------------------------------------------------------------------- /.script/set-robot: -------------------------------------------------------------------------------- 1 | #!/bin/python3 2 | 3 | import sys 4 | import re 5 | import os 6 | 7 | if len(sys.argv) <= 1: 8 | print( 9 | "Error: No arguments provided. Please specify the name of the robot type." 10 | ) 11 | sys.exit(1) 12 | elif len(sys.argv) > 2: 13 | print( 14 | "Error: Too many arguments provided. Please specify only the name of the robot type." 15 | ) 16 | sys.exit(1) 17 | 18 | script_paths = [ 19 | os.path.join(os.getenv("HOME"), "env_setup.bash"), 20 | os.path.join(os.getenv("HOME"), "env_setup.zsh"), 21 | ] 22 | for script_path in script_paths: 23 | try: 24 | with open(script_path, "r") as file: 25 | config = file.read() 26 | updated_config, updated_count = re.subn( 27 | r"export RMCS_ROBOT_TYPE=\"[^\n\"]*\"", 28 | f'export RMCS_ROBOT_TYPE="{sys.argv[1]}"', 29 | config, 30 | ) 31 | if updated_count == 0: 32 | print(f"Error: Cannot find any place to modify in '{script_path}'.") 33 | sys.exit(1) 34 | with open(script_path, "w") as file: 35 | file.write(updated_config) 36 | except FileNotFoundError: 37 | print(f"Error: '{script_path}' not found.") 38 | sys.exit(1) 39 | -------------------------------------------------------------------------------- /.script/ssh-remote: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -z "$1" ]; then 4 | ssh -X remote 5 | else 6 | ssh -t -X remote "$@" 7 | fi 8 | -------------------------------------------------------------------------------- /.script/sync-remote: -------------------------------------------------------------------------------- 1 | #!/bin/python3 2 | 3 | import socket 4 | import selectors 5 | import os 6 | import subprocess 7 | 8 | from colorama import Fore, Style 9 | 10 | 11 | SRC_DIR = "/workspaces/RMCS/rmcs_ws/install" 12 | DST_DIR = "ssh://remote//rmcs_install" 13 | SOCKET_PATH = "/tmp/sync-remote" 14 | 15 | SYNC_FLAG = b"Nothing to do:" 16 | COLORED_SYNC_FLAG = ( 17 | f"{Fore.LIGHTGREEN_EX}{Style.BRIGHT}Nothing to do{Style.RESET_ALL}:".encode("ascii") 18 | ) 19 | 20 | 21 | def create_process(): 22 | command = [ 23 | "unison", 24 | "-ignorearchives", 25 | "-auto", 26 | "-batch", 27 | "-repeat", 28 | "watch", 29 | "-times", 30 | SRC_DIR, 31 | DST_DIR, 32 | "-force", 33 | SRC_DIR, 34 | "-follow", 35 | "Name *", 36 | ] 37 | pipe_r, pipe_w = os.pipe() 38 | process = subprocess.Popen( 39 | command, 40 | stdout=pipe_w, 41 | stderr=pipe_w, 42 | ) 43 | return process, pipe_r 44 | 45 | 46 | def create_server(): 47 | if os.path.exists(SOCKET_PATH): 48 | os.remove(SOCKET_PATH) 49 | 50 | server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) 51 | server.bind(SOCKET_PATH) 52 | server.listen() 53 | server.setblocking(False) 54 | 55 | return server 56 | 57 | 58 | def finish_connection(conn: socket): 59 | try: 60 | conn.sendall(b"f") 61 | except: 62 | pass 63 | finally: 64 | conn.close() 65 | 66 | 67 | def main(): 68 | selector = selectors.DefaultSelector() 69 | 70 | process, pipe_stdout = create_process() 71 | selector.register(pipe_stdout, selectors.EVENT_READ, data=None) 72 | 73 | server = create_server() 74 | selector.register(server, selectors.EVENT_READ, data=None) 75 | 76 | clients = [] 77 | ready = False 78 | 79 | try: 80 | while True: 81 | events = selector.select(timeout=1) 82 | for key, _ in events: 83 | if key.fileobj == server: 84 | conn, _ = server.accept() 85 | if ready: 86 | finish_connection(conn) 87 | else: 88 | clients.append(conn) 89 | else: 90 | stdout = os.read(pipe_stdout, 1024) 91 | if stdout.startswith(SYNC_FLAG): 92 | ready = True 93 | stdout = COLORED_SYNC_FLAG + stdout[len(SYNC_FLAG) :] 94 | for conn in clients: 95 | finish_connection(conn) 96 | clients = [] 97 | else: 98 | ready = False 99 | os.write(1, stdout) 100 | if process.poll() is not None: 101 | break 102 | except KeyboardInterrupt: 103 | try: 104 | process.wait() 105 | except KeyboardInterrupt: 106 | print("SIGINT sent twice, force quitting.") 107 | process.kill() 108 | finally: 109 | server.close() 110 | selector.close() 111 | if os.path.exists(SOCKET_PATH): 112 | os.unlink(SOCKET_PATH) 113 | 114 | 115 | if __name__ == "__main__": 116 | main() 117 | -------------------------------------------------------------------------------- /.script/template/entrypoint: -------------------------------------------------------------------------------- 1 | #!/usr/bin/bash 2 | 3 | # Remove all files in /tmp 4 | rm -rf /tmp/* 5 | 6 | # Remove all files in /run except for /run/dbus 7 | find /run -mindepth 1 -not -path "/run/dbus*" -exec rm -rf {} + 8 | 9 | service rmcs start 10 | 11 | service ssh start 12 | 13 | if [ -f "/etc/avahi/enabled" ]; then 14 | service avahi-daemon start 15 | fi 16 | 17 | sleep infinity -------------------------------------------------------------------------------- /.script/template/env_setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | export ROS_LOCALHOST_ONLY=1 4 | 5 | source /opt/ros/humble/setup.bash 6 | 7 | if [ -f "/rmcs_install/local_setup.bash" ]; then 8 | source /rmcs_install/local_setup.bash 9 | elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.bash" ]; then 10 | source /workspaces/RMCS/rmcs_ws/install/local_setup.bash 11 | fi 12 | 13 | export RMCS_ROBOT_TYPE="" 14 | -------------------------------------------------------------------------------- /.script/template/env_setup.zsh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | export ROS_LOCALHOST_ONLY=1 4 | 5 | source /opt/ros/humble/setup.zsh 6 | 7 | if [ -f "/rmcs_install/local_setup.zsh" ]; then 8 | source /rmcs_install/local_setup.zsh 9 | elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.zsh" ]; then 10 | source /workspaces/RMCS/rmcs_ws/install/local_setup.zsh 11 | fi 12 | 13 | eval "$(register-python-argcomplete3 ros2)" 14 | eval "$(register-python-argcomplete3 colcon)" 15 | 16 | export RMCS_ROBOT_TYPE="" 17 | -------------------------------------------------------------------------------- /.script/template/rmcs-service: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | set -e 4 | 5 | start() { 6 | if [[ -z "$HOME" ]]; then 7 | export HOME=/root 8 | fi 9 | 10 | if is_running; then 11 | echo 'Error: RMCS daemon is still running!' 12 | exit 1 13 | fi 14 | 15 | source ~/env_setup.bash 16 | 17 | if [ -z "${RMCS_ROBOT_TYPE}" ]; then 18 | echo "Error: Please set robot type first (e.g. 'set-robot sentry')." 19 | exit 1 20 | fi 21 | 22 | screen -dmS rmcs bash -c "ros2 launch rmcs_bringup rmcs.launch.py robot:=${RMCS_ROBOT_TYPE}" 23 | 24 | sleep 0.1 25 | 26 | if is_running; then 27 | echo "Successfully started RMCS daemon." 28 | else 29 | echo "Error: Failed to start RMCS daemon." 30 | exit 1 31 | fi 32 | } 33 | 34 | attach() { 35 | screen -r rmcs 36 | } 37 | 38 | stop() { 39 | if is_running; then 40 | screen -S rmcs -X quit 41 | echo "Successfully stopped RMCS daemon." 42 | fi 43 | } 44 | 45 | is_running() { 46 | if ls '/run/screen/S-root' 2>/dev/null | grep -q '.*\.rmcs'; then 47 | return 0 48 | else 49 | return 1 50 | fi 51 | } 52 | 53 | case "$1" in 54 | start) 55 | start 56 | ;; 57 | attach) 58 | attach 59 | ;; 60 | stop) 61 | stop 62 | ;; 63 | restart) 64 | stop 65 | start 66 | ;; 67 | *) 68 | echo 'Usage: service rmcs {start|attach|stop|restart}' 69 | exit 1 70 | ;; 71 | esac 72 | 73 | exit 0 -------------------------------------------------------------------------------- /.script/template/set-hostname: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Check if exactly one argument is passed 4 | if [ "$#" -ne 1 ]; then 5 | echo "Usage: set-hostname " 6 | echo "Example: set-hostname my-sentry" 7 | exit 1 8 | fi 9 | 10 | # Handle the "off" argument 11 | if [ "$1" == "off" ]; then 12 | rm -f /etc/avahi/enabled 13 | service avahi-daemon stop 14 | exit 0 15 | fi 16 | 17 | # Update the avahi-daemon.conf file 18 | AVAHI_CONF="/etc/avahi/avahi-daemon.conf" 19 | if [ -f "$AVAHI_CONF" ]; then 20 | touch /etc/avahi/enabled 21 | sed -i -E 's|^#?host-name=\S*|host-name='"$1"'|' "$AVAHI_CONF" 22 | service avahi-daemon restart 23 | echo "Updated host-name in $AVAHI_CONF to $1." 24 | else 25 | echo "Error: Configuration file $AVAHI_CONF does not exist." 26 | exit 1 27 | fi 28 | -------------------------------------------------------------------------------- /.script/wait-sync: -------------------------------------------------------------------------------- 1 | #!/bin/python3 2 | 3 | import socket 4 | import os 5 | 6 | SOCKET_PATH = "/tmp/sync-remote" 7 | 8 | def wait_for_sync_complete(): 9 | with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as client_socket: 10 | try: 11 | client_socket.connect(SOCKET_PATH) 12 | data = client_socket.recv(1) 13 | if data == b"f": 14 | return 0 15 | elif data == b"": 16 | print(f"Error: Synchronization stopped.") 17 | return 1 18 | else: 19 | print(f"Error: Received unexpected data!") 20 | return 2 21 | except FileNotFoundError: 22 | print("Sync not in progress! Start synchronization with the command 'sync-remote'.") 23 | return 10 24 | except Exception as e: 25 | print(f"Error: {e}") 26 | return 11 27 | 28 | 29 | if __name__ == "__main__": 30 | exit(wait_for_sync_complete()) 31 | -------------------------------------------------------------------------------- /.ssh/config: -------------------------------------------------------------------------------- 1 | Host remote 2 | HostName 127.0.0.1 3 | Port 2022 4 | User root 5 | PreferredAuthentications publickey 6 | IdentityFile ~/.ssh/id_rsa 7 | AddressFamily inet -------------------------------------------------------------------------------- /.vscode/settings.default.json: -------------------------------------------------------------------------------- 1 | { 2 | "clangd.arguments": [ 3 | "--header-insertion=never" 4 | ], 5 | "python.autoComplete.extraPaths": [ 6 | "/opt/ros/humble/lib/python3.10/site-packages", 7 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 8 | ], 9 | "python.analysis.extraPaths": [ 10 | "/opt/ros/humble/lib/python3.10/site-packages", 11 | "/opt/ros/humble/local/lib/python3.10/dist-packages" 12 | ], 13 | "cSpell.words": [ 14 | "ahrs", 15 | "Axisd", 16 | "bringup", 17 | "cboard", 18 | "colcon", 19 | "dbus", 20 | "Eigen", 21 | "GPIO", 22 | "hikcamera", 23 | "imshow", 24 | "librmcs", 25 | "libusb", 26 | "lightbars", 27 | "livox", 28 | "Madgwick's", 29 | "Mayhony's", 30 | "NOLINTNEXTLINE", 31 | "omni", 32 | "PLUGINLIB", 33 | "Quaterniond", 34 | "rclcpp", 35 | "rclpy", 36 | "respawn", 37 | "RMCS", 38 | "RMUC", 39 | "RMUL", 40 | "Robo", 41 | "rosdep", 42 | "rviz", 43 | "setpoint", 44 | "sourcedir", 45 | "supercap", 46 | "uart", 47 | "UGAS", 48 | "urdf", 49 | "vruntime", 50 | "xacro" 51 | ] 52 | } -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # 23.0 is the minimum docker engine version that required to build. 2 | # If your docker engine lower than this version, please upgrade it or manually enable the BuildKit. 3 | 4 | 5 | 6 | # Base container, provides a runtime environment 7 | FROM ros:humble AS rmcs-base 8 | 9 | # Change bash as default shell instead of sh 10 | SHELL ["/bin/bash", "-c"] 11 | 12 | # Set timezone and non-interactive mode 13 | ENV TZ=Asia/Shanghai \ 14 | DEBIAN_FRONTEND=noninteractive 15 | 16 | # Install tools and libraries. 17 | RUN apt-get update && apt-get -y install \ 18 | vim wget curl unzip \ 19 | zsh screen \ 20 | usbutils net-tools iputils-ping \ 21 | libusb-1.0-0-dev \ 22 | libeigen3-dev \ 23 | libopencv-dev \ 24 | libgoogle-glog-dev \ 25 | libgflags-dev \ 26 | libatlas-base-dev \ 27 | libsuitesparse-dev \ 28 | libceres-dev \ 29 | ros-humble-rviz2 \ 30 | && rm -rf /var/lib/apt/lists/* 31 | 32 | # Install openvino 2024.6 runtime 33 | RUN wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ 34 | apt-key add ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ 35 | rm ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ 36 | echo "deb https://apt.repos.intel.com/openvino/2024 ubuntu22 main" > /etc/apt/sources.list.d/intel-openvino-2024.list && \ 37 | apt-get update && \ 38 | apt-get install -y openvino-2024.6.0 && \ 39 | rm -rf /var/lib/apt/lists/* 40 | 41 | # Install Livox SDK 42 | RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git && \ 43 | cd ./Livox-SDK2/ && \ 44 | mkdir build && cd build && \ 45 | cmake .. && make -j && \ 46 | make install && \ 47 | cd ../.. && rm -rf ./Livox-SDK2/ 48 | 49 | # Mount rmcs source and install dependencies 50 | RUN --mount=type=bind,target=/tmp/rmcs_ws/src,source=rmcs_ws/src,readonly \ 51 | apt-get update && \ 52 | rosdep install --from-paths /tmp/rmcs_ws/src --ignore-src -r -y && \ 53 | rm -rf /var/lib/apt/lists/* 54 | 55 | # Install unison to allow file synchronization 56 | RUN cd /tmp && \ 57 | wget -O unison.tar.gz https://github.com/bcpierce00/unison/releases/download/v2.53.7/unison-2.53.7-ubuntu-x86_64.tar.gz && \ 58 | mkdir -p unison && tar -zxf unison.tar.gz -C unison && \ 59 | cp unison/bin/* /usr/local/bin && \ 60 | rm -rf unison unison.tar.gz 61 | 62 | 63 | 64 | # Developing container, works with devcontainer 65 | FROM rmcs-base AS rmcs-develop 66 | 67 | # Install develop tools 68 | RUN apt-get update && apt-get -y install \ 69 | libc6-dev gcc-12 g++-12 \ 70 | cmake make ninja-build \ 71 | openssh-client \ 72 | lsb-release software-properties-common gnupg sudo \ 73 | python3-colorama python3-dpkt && \ 74 | wget -O ./llvm-snapshot.gpg.key https://apt.llvm.org/llvm-snapshot.gpg.key && \ 75 | apt-key add ./llvm-snapshot.gpg.key && \ 76 | rm ./llvm-snapshot.gpg.key && \ 77 | echo "deb https://apt.llvm.org/jammy/ llvm-toolchain-jammy main" > /etc/apt/sources.list.d/llvm-apt.list && \ 78 | apt-get update && \ 79 | version=`apt-cache search clangd- | grep clangd- | awk -F' ' '{print $1}' | sort -V | tail -1 | cut -d- -f2` && \ 80 | apt-get install -y clangd-$version && \ 81 | update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-12 50 && \ 82 | update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-12 50 && \ 83 | update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-$version 50 && \ 84 | rm -rf /var/lib/apt/lists/* 85 | 86 | # Add user 87 | RUN useradd -m developer --shell /bin/zsh && echo "developer:developer" | chpasswd && adduser developer sudo && \ 88 | echo "developer ALL=(ALL:ALL) NOPASSWD:ALL" >> /etc/sudoers && \ 89 | gpasswd --add developer dialout 90 | WORKDIR /home/developer 91 | ENV USER=developer 92 | ENV WORKDIR=/home/developer 93 | 94 | # Generate/load ssh key and setup unison 95 | RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh \ 96 | mkdir -p .ssh && \ 97 | if [ ! -f "/tmp/.ssh/id_rsa" ]; then ssh-keygen -N "" -f "/tmp/.ssh/id_rsa"; fi && \ 98 | cp -r /tmp/.ssh/* .ssh && \ 99 | chown -R 1000:1000 .ssh && chmod 600 .ssh/id_rsa && \ 100 | mkdir -p .unison && \ 101 | echo 'confirmbigdel = false' >> ".unison/default.prf" && \ 102 | chown -R 1000:1000 .unison 103 | 104 | USER developer 105 | 106 | # Install oh my zsh, change theme to af-magic and setup environment of zsh 107 | RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)" && \ 108 | sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' ~/.zshrc && \ 109 | echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ 110 | echo 'export PATH=${PATH}:/workspaces/RMCS/.script' >> ~/.zshrc 111 | 112 | # Copy environment setup scripts 113 | COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash 114 | COPY --chown=1000:1000 .script/template/env_setup.zsh env_setup.zsh 115 | 116 | 117 | 118 | # Runtime container, will automatically launch the main program 119 | FROM rmcs-base AS rmcs-runtime 120 | 121 | # Install runtime tools 122 | RUN apt-get update && \ 123 | apt-get install -y tini openssh-server avahi-daemon && \ 124 | rm -rf /var/lib/apt/lists/* && \ 125 | echo 'Port 2022' >> /etc/ssh/sshd_config && \ 126 | echo 'PermitRootLogin yes' >> /etc/ssh/sshd_config && \ 127 | echo 'PasswordAuthentication no' >> /etc/ssh/sshd_config && \ 128 | sed -i 's/#enable-dbus=yes/enable-dbus=no/g' /etc/avahi/avahi-daemon.conf 129 | 130 | # Install oh my zsh, disable plugins and update, setup environment and set zsh as default shell 131 | RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)" && \ 132 | sed -i 's/plugins=(git)/plugins=()/g' ~/.zshrc && \ 133 | sed -i "s/# zstyle ':omz:update' mode disabled/zstyle ':omz:update' mode disabled/g" ~/.zshrc && \ 134 | echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ 135 | echo 'export PATH=${PATH}:/rmcs_install/lib/rmcs_cli' >> ~/.zshrc && \ 136 | chsh -s /bin/zsh root 137 | 138 | RUN mkdir -p /rmcs_install/ 139 | 140 | COPY --chown=root:root .script/set-robot /usr/local/bin/set-robot 141 | COPY --chown=root:root .script/template/set-hostname /usr/local/bin/set-hostname 142 | 143 | COPY --chown=root:root .script/template/entrypoint /entrypoint 144 | COPY --chown=root:root .script/template/rmcs-service /etc/init.d/rmcs 145 | 146 | COPY --from=rmcs-develop --chown=root:root /home/developer/.ssh/id_rsa.pub /root/.ssh/authorized_keys 147 | 148 | WORKDIR /root/ 149 | COPY --chown=root:root .script/template/env_setup.bash env_setup.bash 150 | COPY --chown=root:root .script/template/env_setup.zsh env_setup.zsh 151 | 152 | ENTRYPOINT ["tini", "--"] 153 | CMD [ "/entrypoint" ] -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RMCS 2 | RoboMaster Control System based on ROS2. 3 | 4 | ## Development 5 | 6 | ### Pre-requirements: 7 | 8 | - x86-64 架构 9 | - 任意 Linux 发行版，或 WSL2（参见 [WSL2开发指南](docs/zh-cn/wsl2_develop_guide.md)） 10 | - [VSCode](https://code.visualstudio.com/)，安装 [Dev Containers 扩展](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) 11 | - [安装 Docker 并 配置代理（部分国家或地区）](docs/zh-cn/docker_with_proxy.md) 12 | 13 | ### Step 1：获取镜像 14 | 15 | 下载开发镜像： 16 | ```bash 17 | docker pull qzhhhi/rmcs-develop:latest 18 | ``` 19 | 20 | 也可自行使用 `Dockerfile` 构建，参见 [镜像构建指南](docs/zh-cn/build_docker_image.md)。 21 | 22 | ### Step 2：克隆并打开仓库 23 | 24 | 克隆仓库，注意需要使用 `recurse-submodules` 以克隆子模块： 25 | 26 | ```bash 27 | git clone --recurse-submodules https://github.com/Alliance-Algorithm/RMCS.git 28 | ``` 29 | 30 | 在 VSCode 中打开仓库： 31 | 32 | ```bash 33 | code ./RMCS 34 | ``` 35 | 36 | 按 `Ctrl+Shift+P`，在弹出的菜单中选择 `Dev Containers: Reopen in Container`。 37 | 38 | VSCode 将拉起一个 `Docker` 容器，容器中已配置好完整开发环境，之后所有工作将在容器内进行。 39 | 40 | 如果 `Dev Containers` 在启动时卡住很长一段时间，可以尝试 [这个解决方案](docs/zh-cn/fix_devcontainer_stuck.md)。 41 | 42 | ### Step 3：配置 VSCode 43 | 44 | 在 VSCode 中新建终端，输入： 45 | 46 | ```bash 47 | cp .vscode/settings.default.json .vscode/settings.json 48 | ``` 49 | 50 | 这会应用我们推荐的 VSCode 配置文件，你也可以按需自行修改配置文件。 51 | 52 | 在拓展列表中，可以看到我们推荐使用的拓展正在安装，你也可以按需自行删减拓展。 53 | 54 | ### Step 4：构建 55 | 56 | 在 VSCode 终端中输入： 57 | 58 | ```bash 59 | build-rmcs 60 | ``` 61 | 62 | 将会运行 `.script/build-rmcs` 脚本，在路径 `rmcs_ws` 下开始构建代码。 63 | 64 | 构建完毕后，基于 `clangd` 的 `C++` 代码提示将可用。此时可以正常编写代码。 65 | 66 | Note: 用于开发的所有脚本均位于 `.script` 中，参见 开发脚本手册(TODO)。 67 | 68 | ### Step 5 (Optional)：运行 69 | 70 | 编写代码并编译完成后，可以使用： 71 | 72 | ```bash 73 | launch-rmcs 74 | ``` 75 | 76 | 在本机上运行代码。在首次运行代码前，需要调用 `set-robot` 脚本设置机器人类型。 77 | 78 | #### 确认设备接入 79 | 80 | 可以使用 `lsusb` 确定 [下位机](https://github.com/Alliance-Algorithm/rmcs_slave) 是否已接入，若已接入，则 `lsusb` 输出类似： 81 | 82 | ``` 83 | Bus 001 Device 004: ID a11c:75f3 Alliance RoboMaster Team. RMCS Slave v2.1.2 84 | ``` 85 | 86 | 在 WSL2 下，需要 [使用 usbipd 对设备进行转接](docs/zh-cn/wsl2_develop_guide.md#step-5-optional)。 87 | 88 | #### 确认权限正确 89 | 90 | 在主机（不要在 `docker` 容器）的终端中输入： 91 | 92 | ```bash 93 | echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="a11c", MODE="0666"' | sudo tee /etc/udev/rules.d/95-rmcs-slave.rules && 94 | sudo udevadm control --reload-rules && 95 | sudo udevadm trigger 96 | ``` 97 | 98 | 以允许非 root 用户读写 RMCS 下位机，此指令只需执行一次。 99 | 100 | ## Deployment 101 | 102 | ### Pre-requirements: 103 | 104 | - x86-64 架构 105 | - 任意 Linux 发行版 106 | - [安装 Docker](docs/zh-cn/docker_with_proxy.md#ubuntu-安装-docker) 107 | 108 | ### Step 1：获取镜像 109 | 110 | 下载部署镜像： 111 | 112 | ```bash 113 | docker pull qzhhhi/rmcs-develop:latest 114 | ``` 115 | 116 | 如果不方便在 MiniPC 上配置代理，可以在开发机上下载镜像后，使用 117 | 118 | ```bash 119 | docker save qzhhhi/rmcs-runtime:latest > rmcs-runtime.tar 120 | ``` 121 | 122 | 然后使用任意方式（如 scp）将 `rmcs-runtime.tar` 传送到 MiniPC 上，并在其上执行： 123 | 124 | ```bash 125 | docker load -i rmcs-runtime.tar 126 | ``` 127 | 128 | 即获取部署镜像。 129 | 130 | ### Step 2：启动容器 131 | 132 | 在 MiniPC 终端中输入： 133 | 134 | ```bash 135 | docker run -d --restart=always --privileged --network=host -v /dev:/dev qzhhhi/rmcs-runtime:latest 136 | ``` 137 | 138 | 即可启动部署镜像，此后镜像将保持开机自启。 139 | 140 | ### Step 3：远程连接 141 | 142 | 在开发容器终端中输入： 143 | 144 | ```bash 145 | set-remote 146 | ``` 147 | 148 | 其中，`remote-host` 可以为 MiniPC 的： 149 | 150 | 1. IPv4 / IPv6 地址 (e.g., 169.254.233.233) 151 | 152 | 2. IPv4 / IPv6 Link-local 地址 (e.g., fe80::c6d0:e3ff:fed7:ed12%eth0) 153 | 154 | 3. mDNS 主机名 (e.g., my-sentry.local) 155 | 156 | 参见 网络配置指南(TODO)。 157 | 158 | 接下来在开发容器终端中继续输入： 159 | 160 | ```bash 161 | ssh-remote 162 | ``` 163 | 164 | 即可在开发容器中，ssh 连接到远程的部署容器。 165 | 166 | **RMCS 的所有代码更新和调试，都基于从开发容器向部署容器的 ssh 连接。** 167 | 168 | 部署容器会监听 TCP:2022 端口作为 ssh-server 端口，请注意保证端口空闲。 169 | 170 | 参见 容器设计思想(TODO)。 171 | 172 | > Tip: GUI 可以从部署容器中穿出，尝试在 ssh-remote 中打开 rviz2。 173 | 174 | ### Step 4：同步构建产物 175 | 176 | 新启动的部署容器内是没有代码的，需要由开发容器上传。 177 | 178 | 在开发容器中构建完成后，可以执行指令： 179 | 180 | ```bash 181 | sync-remote 182 | ``` 183 | 184 | 这将拉起一个同步进程，自动将开发容器中的构建产物同步到部署容器。 185 | 186 | 同步进程除非主动使用 `Ctrl+C` 结束，否则不会退出，其会监视所有文件变更，并实时同步到部署容器。 187 | 188 | > Tip: 由于 `build-rmcs` 采用 `symlink-install` 方式构建，因此对于配置文件和 .py 文件，直接修改其源文件，无需编译即可触发同步。 189 | 190 | ### Step 5：重启服务 191 | 192 | RMCS 在部署容器中以服务方式启动 (`/etc/init.d/rmcs`)。 193 | 194 | 确认构建产物同步完毕后（以出现 `Nothing to do` 为标志），进入 `ssh-remote`，输入： 195 | 196 | ```bash 197 | set-robot 198 | ``` 199 | 200 | 设置启动的机器人类型（例如 set-robot sentry）。 201 | 202 | 接下来继续输入： 203 | 204 | ```bash 205 | service rmcs restart 206 | ``` 207 | 208 | 如果一切正常，其将会输出： 209 | 210 | ```bash 211 | Successfully stopped RMCS daemon. 212 | Successfully started RMCS daemon. 213 | ``` 214 | 215 | 这证明 RMCS 已成功在部署容器中启动，并会随以后的每次容器启动而启动。 216 | 217 | 接下来可以使用： 218 | 219 | ```bash 220 | service rmcs attach 221 | ``` 222 | 223 | 查看 RMCS 的实时输出。 224 | 225 | > 需要注意的是，`service rmcs attach` 本质上是连接到了一个 `GNU screen` 会话，因此任何按键都会被忠实地转发至 RMCS。例如，当键入 `Ctrl+C` 时，RMCS 会接到 `SIGINT`，从而停止运行。 226 | > 227 | > 如果希望退出对实时输出的查看，可以键入 `Ctrl+A` ，然后按 `D`。 228 | > 229 | > 如果希望向上翻页，可以键入 `Ctrl+A` ，然后按 `Esc`。 230 | > 231 | > 更多快捷键组合参见 [Screen Quick Reference](https://gist.github.com/andrimanna/e5379fe6db3af0ecdb1e49e8cfb74d24)。 232 | 233 | ### Step 6：糖 234 | 235 | 指令 `ssh-remote` 后可以添加参数，参数内容即为建立链接后立即执行的指令。 236 | 237 | 例如： 238 | 239 | ```bash 240 | ssh-remote service rmcs restart 241 | ``` 242 | 243 | 可以在开发容器端快速重启部署容器中的 RMCS。 244 | 245 | 又如： 246 | 247 | ```bash 248 | ssh-remote service rmcs attach 249 | ``` 250 | 251 | 可以在开发容器端快速查看部署容器中 RMCS 的实时输出。 252 | 253 | 实际上，可以使用指令： 254 | 255 | ```bash 256 | attach-remote 257 | ``` 258 | 259 | 作为前者的替代（其实现就是前者）。 260 | 261 | 进一步的，还可以使用： 262 | 263 | ```bash 264 | attach-remote -r 265 | ``` 266 | 267 | 作为 `ssh-remote "service rmcs restart && service rmcs attach"` 的替代。 268 | 269 | 其在重启 RMCS 守护进程后，自动连接显示实时输出。 270 | 271 | 更进一步的，指令间还可以组合，例如： 272 | 273 | ```bash 274 | build-rmcs && wait-sync && attach-remote -r 275 | ``` 276 | 277 | 可以触发 RMCS 构建，`wait-sync` 等待文件同步完成，接下来重启 RMCS 守护进程后，显示实时输出。 278 | -------------------------------------------------------------------------------- /docs/zh-cn/build_docker_image.md: -------------------------------------------------------------------------------- 1 | # Build Docker Image 2 | 3 | 于 Repo 根目录下，在终端中分别使用： 4 | 5 | ```bash 6 | docker build . --target rmcs-runtime -t rmcs-runtime:latest 7 | ``` 8 | 9 | ```bash 10 | docker build . --target rmcs-develop -t rmcs-develop:latest 11 | ``` 12 | 13 | 构建开发容器和部署容器。 14 | 15 | 需要注意的是，部分国家和地区会阻断对 `github` 的连接，此时构建需要使用代理： 16 | 17 | ```bash 18 | docker build . --target rmcs-runtime -t rmcs-runtime:latest \ 19 | --network host \ 20 | --build-arg HTTP_PROXY=http://127.0.0.1:7890 \ 21 | --build-arg HTTPS_PROXY=http://127.0.0.1:7890 22 | ``` 23 | 24 | ```bash 25 | docker build . --target rmcs-develop -t rmcs-develop:latest \ 26 | --network host \ 27 | --build-arg HTTP_PROXY=http://127.0.0.1:7890 \ 28 | --build-arg HTTPS_PROXY=http://127.0.0.1:7890 29 | ``` 30 | 31 | 请自行把 `http://127.0.0.1:7890` 改为合适的代理地址。 32 | 33 | 如果不使用本机回环地址 (127.0.0.1) 作为代理地址，构建参数中 `--network host` 可以省去。 -------------------------------------------------------------------------------- /docs/zh-cn/docker_with_proxy.md: -------------------------------------------------------------------------------- 1 | # Docker With Proxy 2 | 3 | ### Ubuntu 安装 Docker 4 | 5 | 在终端中运行： 6 | 7 | ```bash 8 | sudo apt-get update && 9 | sudo apt-get -y install ca-certificates curl && 10 | sudo install -m 0755 -d /etc/apt/keyrings && 11 | sudo curl -fsSL https://mirrors.aliyun.com/docker-ce/linux/ubuntu/gpg -o /etc/apt/keyrings/docker.asc && 12 | sudo chmod a+r /etc/apt/keyrings/docker.asc && 13 | echo \ 14 | "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.asc] https://mirrors.aliyun.com/docker-ce/linux/ubuntu \ 15 | $(. /etc/os-release && echo "$VERSION_CODENAME") stable" | \ 16 | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null && 17 | sudo apt-get update && 18 | sudo apt-get -y install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin 19 | ``` 20 | 21 | ### 设置无需 sudo 22 | 23 | ```bash 24 | sudo usermod -aG docker $USER 25 | ``` 26 | 27 | 完成后需要重新登录。 28 | 29 | ### 设置 docker pull 代理 30 | 31 | ```bash 32 | sudo mkdir -p /etc/systemd/system/docker.service.d && 33 | sudo tee /etc/systemd/system/docker.service.d/proxy.conf < 检查Windows版本：按 Win + R 输入 `winver` 回车即可显示。 6 | 7 | **对于较低版本的 Windows 使用者，建议安装双系统进行开发。** 8 | 9 | 如果您有在其它版本的 Windows 中优雅的使用 WSL 开发 RMCS 的诀窍，可以向我们提出 Issue 或 Pull Request。 10 | 11 | ## 12 | 13 | ### Step 1 14 | 在设置 `系统 > 开发者选项` 中，找到 `启用 sudo` 选项，将其开启。 15 | 16 | ### Step 2 17 | 18 | 在 Windows 的用户目录（通常为 `C:\Users\`）中，新建文件 `.wslconfig`，填充以下内容： 19 | 20 | ```ini 21 | [experimental] 22 | networkingMode=mirrored 23 | dnsTunneling=true 24 | firewall=true 25 | autoProxy=true 26 | ``` 27 | 28 | 配置此设置以启用镜像模式网络。 29 | 30 | > 在运行 Windows 11 22H2 及更高版本的计算机上，可以启用 [**镜像模式网络**](https://learn.microsoft.com/en-us/windows/wsl/networking#mirrored-mode-networking)。 31 | > 32 | > 启用此功能会将 WSL 更改为全新的网络体系结构，其目标是将 Windows 上的网络接口“镜像”到 Linux 中，以添加新的网络功能并提高兼容性。 33 | 34 | ### Step 3 35 | 36 | 若 WSL 未安装，则打开 Windows 终端，输入 37 | 38 | ```powershell 39 | sudo wsl --install 40 | ``` 41 | 42 | 此命令将启用运行最新版 WSL2，并安装 Ubuntu 发行版。 43 | 44 | 若 WSL 已安装，则输入 45 | 46 | ```powershell 47 | wsl --shutdown 48 | ``` 49 | 50 | 和 51 | 52 | ```powershell 53 | wsl 54 | ``` 55 | 56 | 重启 WSL 以应用更改。 57 | 58 | ### Step 4 59 | 60 | [安装 docker 并配置代理（部分国家或地区）](docker_with_proxy.md)。 61 | 62 | 不建议安装 Docker Desktop。 63 | 64 | ### Step 5 (Optional) 65 | 66 | 在 Windows 终端中输入： 67 | 68 | ```powershell 69 | winget install --interactive --exact dorssel.usbipd-win 70 | ``` 71 | 72 | 这将安装（过程可能需要重启）： 73 | 74 | - 名为 usbipd 的服务（显示名称：USBIP 设备主机）。 可使用 Windows 中的“服务”应用检查此服务的状态。 75 | - 命令行工具 usbipd。 此工具的位置将添加到 PATH 环境变量。 76 | - 名为 usbipd 的防火墙规则，用于允许所有本地子网连接到服务。 可修改此防火墙规则以微调访问控制。 77 | 78 | 完成后，将 [下位机](https://github.com/Alliance-Algorithm/rmcs_slave) 接入PC，在终端中输入： 79 | 80 | ```powershell 81 | usbipd list 82 | ``` 83 | 84 | 如果一切正常，你将得到类似如下的回应： 85 | ``` 86 | Connected: 87 | BUSID VID:PID DEVICE STATE 88 | 1-3 a11c:75f3 RMCS Slave v2.1.2 Not shared 89 | ... 90 | ``` 91 | 92 | 复制显示的设备 BUSID（示例里为 `1-3`），使用命令共享设备（修改 `` 为复制的值）： 93 | 94 | ```powershell 95 | sudo usbipd bind --busid 96 | ``` 97 | 98 | 再次使用命令 `usbipd list` 检查设备状态，可以发现其 `STATE` 栏的内容变为 `Shared` 。 99 | 100 | 接下来使用命令将设备附加到 WSL： 101 | 102 | ```powershell 103 | usbipd attach --wsl --busid 104 | ``` 105 | 106 | 此时应该可以在 WSL 中使用 `lsusb` 或类似的指令检查到下位机设备。 107 | 108 | ``` 109 | Bus 001 Device 004: ID a11c:75f3 Alliance RoboMaster Team. RMCS Slave v2.1.2 110 | ``` 111 | 112 | 对于接入同一位置的同个下位机，`usbipd bind` 指令只需执行一次。 113 | 114 | 但每次将设备拔出再插入后，需要再次执行 `usbipd attach` 指令，才能再次在 WSL 中访问。 115 | 116 | 在 WSL 中完成设备使用后，可物理断开 USB 设备，或运行此命令： 117 | 118 | ```powershell 119 | usbipd detach --busid 120 | ``` 121 | 122 | 123 | ## References 124 | 125 | - [Install WSL | Microsoft Learn](https://learn.microsoft.com/en-us/windows/wsl/install) 126 | 127 | - [Accessing network applications with WSL | Microsoft Learn](https://learn.microsoft.com/en-us/windows/wsl/networking#mirrored-mode-networking) 128 | 129 | - [Connect USB devices | Microsoft Learn](https://learn.microsoft.com/en-us/windows/wsl/connect-usb) 130 | 131 | - [What version of Windows am I running? | Microsoft Learn](https://learn.microsoft.com/en-us/windows/client-management/client-tools/windows-version-search) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(rmcs_bringup) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | 13 | # uncomment the following section in order to fill in 14 | # further dependencies manually. 15 | # find_package( REQUIRED) 16 | install( 17 | DIRECTORY config launch 18 | DESTINATION share/${PROJECT_NAME} 19 | ) 20 | 21 | install( 22 | DIRECTORY config launch 23 | DESTINATION share/${PROJECT_NAME} 24 | ) 25 | 26 | if(BUILD_TESTING) 27 | find_package(ament_lint_auto REQUIRED) 28 | 29 | # the following line skips the linter which checks for copyrights 30 | # comment the line when a copyright and license is added to all source files 31 | set(ament_cmake_copyright_FOUND TRUE) 32 | 33 | # the following line skips cpplint (only works in a git repo) 34 | # comment the line when this package is in a git repo and when 35 | # a copyright and license is added to all source files 36 | set(ament_cmake_cpplint_FOUND TRUE) 37 | ament_lint_auto_find_test_dependencies() 38 | endif() 39 | 40 | ament_package() 41 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/config/auto_aim_test.yaml: -------------------------------------------------------------------------------- 1 | rmcs_executor: 2 | ros__parameters: 3 | update_rate: 1000.0 4 | components: 5 | - rmcs_core::hardware::Infantry -> infantry_hardware 6 | - rmcs_core::referee::Status -> referee_status 7 | 8 | - rmcs_core::controller::gimbal::GimbalController -> gimbal_controller 9 | - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller 10 | - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller 11 | - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller 12 | - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller 13 | 14 | - rmcs_core::controller::gimbal::ShootingController -> shooting_controller 15 | - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller 16 | - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller 17 | - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller 18 | 19 | - rmcs_core::controller::chassis::ChassisController -> chassis_controller 20 | - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller 21 | 22 | - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster 23 | 24 | - rmcs_core::referee::command::Interaction -> referee_interaction 25 | - rmcs_core::referee::command::interaction::Ui -> referee_ui 26 | - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry 27 | 28 | - rmcs_core::referee::Command -> referee_command 29 | 30 | - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer 31 | - rmcs_auto_aim::AutoAimController -> auto_aim_controller 32 | 33 | infantry_hardware: 34 | ros__parameters: 35 | usb_pid: -1 36 | yaw_motor_zero_point: 3387 37 | pitch_motor_zero_point: 6716 38 | 39 | referee_status: 40 | ros__parameters: 41 | path: /dev/ttyUSB0 42 | 43 | gimbal_controller: 44 | ros__parameters: 45 | upper_limit: -0.4598 46 | lower_limit: 0.4362 47 | 48 | yaw_angle_pid_controller: 49 | ros__parameters: 50 | measurement: /gimbal/yaw/control_angle_error 51 | control: /gimbal/yaw/control_velocity 52 | kp: 15.0 53 | ki: 0.0 54 | kd: 0.0 55 | 56 | yaw_velocity_pid_controller: 57 | ros__parameters: 58 | measurement: /gimbal/yaw/velocity_imu 59 | setpoint: /gimbal/yaw/control_velocity 60 | control: /gimbal/yaw/control_torque 61 | kp: 3.5 62 | ki: 0.0 63 | kd: 0.0 64 | 65 | pitch_angle_pid_controller: 66 | ros__parameters: 67 | measurement: /gimbal/pitch/control_angle_error 68 | control: /gimbal/pitch/control_velocity 69 | kp: 20.00 70 | ki: 0.0 71 | kd: 0.0 72 | 73 | pitch_velocity_pid_controller: 74 | ros__parameters: 75 | measurement: /gimbal/pitch/velocity_imu 76 | setpoint: /gimbal/pitch/control_velocity 77 | control: /gimbal/pitch/control_torque 78 | kp: 0.8 79 | ki: 0.0 80 | kd: 0.0 81 | 82 | shooting_controller: 83 | ros__parameters: 84 | friction_wheels: 85 | - /gimbal/left_friction 86 | - /gimbal/right_friction 87 | friction_velocities: 88 | - 740.0 89 | - 740.0 90 | is_42mm: false 91 | bullets_per_feeder_turn: 8.0 92 | shot_frequency: 20.0 93 | safe_shot_frequency: 10.0 94 | eject_frequency: 10.0 95 | eject_time: 0.05 96 | deep_eject_frequency: 5.0 97 | deep_eject_time: 0.2 98 | 99 | left_friction_velocity_pid_controller: 100 | ros__parameters: 101 | measurement: /gimbal/left_friction/velocity 102 | setpoint: /gimbal/left_friction/control_velocity 103 | control: /gimbal/left_friction/control_torque 104 | kp: 0.003436926 105 | ki: 0.00 106 | kd: 0.009373434 107 | 108 | right_friction_velocity_pid_controller: 109 | ros__parameters: 110 | measurement: /gimbal/right_friction/velocity 111 | setpoint: /gimbal/right_friction/control_velocity 112 | control: /gimbal/right_friction/control_torque 113 | kp: 0.003436926 114 | ki: 0.00 115 | kd: 0.009373434 116 | 117 | bullet_feeder_velocity_pid_controller: 118 | ros__parameters: 119 | measurement: /gimbal/bullet_feeder/velocity 120 | setpoint: /gimbal/bullet_feeder/control_velocity 121 | control: /gimbal/bullet_feeder/control_torque 122 | kp: 0.583 123 | ki: 0.0 124 | kd: 0.0 125 | 126 | left_front_wheel_velocity_pid_controller: 127 | ros__parameters: 128 | measurement: /chassis/left_front_wheel/velocity 129 | setpoint: /chassis/left_front_wheel/control_velocity 130 | control: /chassis/left_front_wheel/control_torque_unrestricted 131 | kp: 0.185 132 | ki: 0.00 133 | kd: 0.00 134 | 135 | left_back_wheel_velocity_pid_controller: 136 | ros__parameters: 137 | measurement: /chassis/left_back_wheel/velocity 138 | setpoint: /chassis/left_back_wheel/control_velocity 139 | control: /chassis/left_back_wheel/control_torque_unrestricted 140 | kp: 0.185 141 | ki: 0.00 142 | kd: 0.00 143 | 144 | right_back_wheel_velocity_pid_controller: 145 | ros__parameters: 146 | measurement: /chassis/right_back_wheel/velocity 147 | setpoint: /chassis/right_back_wheel/control_velocity 148 | control: /chassis/right_back_wheel/control_torque_unrestricted 149 | kp: 0.185 150 | ki: 0.00 151 | kd: 0.00 152 | 153 | right_front_wheel_velocity_pid_controller: 154 | ros__parameters: 155 | measurement: /chassis/right_front_wheel/velocity 156 | setpoint: /chassis/right_front_wheel/control_velocity 157 | control: /chassis/right_front_wheel/control_torque_unrestricted 158 | kp: 0.185 159 | ki: 0.00 160 | kd: 0.00 161 | 162 | chassis_power_controller: 163 | ros__parameters: 164 | motors: 165 | - /chassis/left_front_wheel 166 | - /chassis/left_back_wheel 167 | - /chassis/right_back_wheel 168 | - /chassis/right_front_wheel 169 | 170 | auto_aim_controller: 171 | ros__parameters: 172 | # capture 173 | use_video: false # If true, use video stream instead of camera. 174 | video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" 175 | exposure_time: 3 176 | invert_image: false 177 | # identifier 178 | armor_model_path: "/models/mlp.onnx" 179 | # pnp 180 | fx: 1.722231837421459e+03 181 | fy: 1.724876404292754e+03 182 | cx: 7.013056440882832e+02 183 | cy: 5.645821718351237e+02 184 | k1: -0.064232403853946 185 | k2: -0.087667493884102 186 | k3: 0.792381808294582 187 | #transform quaternion 188 | q_w: 0.0 189 | q_x: 0.0 190 | q_y: 0.0 191 | q_z: 1.0 192 | # tracker 193 | armor_predict_duration: 500 194 | # controller 195 | gimbal_predict_duration: 100 196 | yaw_error: 0.02 197 | pitch_error: -0.01 198 | shoot_velocity: 26.0 199 | predict_sec: 7.0 200 | # etc 201 | buff_predict_duration: 200 202 | buff_model_path: "/models/buff_nocolor_v6.onnx" 203 | omni_exposure: 1000.0 204 | record_fps: 120 205 | debug: false # Setup in actual using.Debug mode is used when referee is not ready 206 | debug_color: 0 # 0 For blue while 1 for red. mine 207 | debug_robot_id: 4 208 | debug_buff_mode: false 209 | record: false 210 | raw_img_pub: false # Set false in actual use 211 | image_viewer_type: 3 -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/config/hero.yaml: -------------------------------------------------------------------------------- 1 | rmcs_executor: 2 | ros__parameters: 3 | update_rate: 1000.0 4 | components: 5 | - rmcs_core::hardware::Hero -> hero_hardware 6 | 7 | - rmcs_core::referee::Status -> referee_status 8 | - rmcs_core::referee::command::Interaction -> referee_interaction 9 | - rmcs_core::referee::command::interaction::Ui -> referee_ui 10 | - rmcs_core::referee::app::ui::Hero -> referee_ui_hero 11 | - rmcs_core::referee::Command -> referee_command 12 | 13 | - rmcs_core::controller::gimbal::GimbalController -> gimbal_controller 14 | - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller 15 | - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller 16 | 17 | - rmcs_core::controller::gimbal::ShootingController -> shooting_controller 18 | - rmcs_core::controller::pid::PidController -> second_left_friction_velocity_pid_controller 19 | - rmcs_core::controller::pid::PidController -> first_left_friction_velocity_pid_controller 20 | - rmcs_core::controller::pid::PidController -> first_right_friction_velocity_pid_controller 21 | - rmcs_core::controller::pid::PidController -> second_right_friction_velocity_pid_controller 22 | - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller 23 | 24 | - rmcs_core::controller::chassis::ChassisController -> chassis_controller 25 | - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller 26 | 27 | # - rmcs_core::controller::gimbal::ShootingRecorder -> shooting_recorder 28 | 29 | - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer 30 | - rmcs_auto_aim::AutoAimController -> auto_aim_controller 31 | 32 | hero_hardware: 33 | ros__parameters: 34 | usb_pid_top_board: 0x652f 35 | usb_pid_bottom_board: 0xc1d3 36 | yaw_motor_zero_point: 27555 37 | pitch_motor_zero_point: 24514 38 | external_imu_port: /dev/ttyUSB0 39 | 40 | gimbal_controller: 41 | ros__parameters: 42 | upper_limit: -0.4598 43 | lower_limit: 0.4362 44 | 45 | yaw_angle_pid_controller: 46 | ros__parameters: 47 | measurement: /gimbal/yaw/control_angle_error 48 | control: /gimbal/yaw/control_velocity 49 | kp: 16.0 50 | ki: 0.0 51 | kd: 0.0 52 | 53 | pitch_angle_pid_controller: 54 | ros__parameters: 55 | measurement: /gimbal/pitch/control_angle_error 56 | control: /gimbal/pitch/control_velocity 57 | kp: 16.00 58 | ki: 0.0 59 | kd: 0.0 60 | 61 | shooting_controller: 62 | ros__parameters: 63 | friction_wheels: 64 | - /gimbal/first_left_friction 65 | - /gimbal/first_right_friction 66 | - /gimbal/second_left_friction 67 | - /gimbal/second_right_friction 68 | friction_velocities: 69 | - 530.85 70 | - 530.85 71 | - 530.70 72 | - 530.70 73 | is_42mm: true 74 | bullets_per_feeder_turn: 6.0 75 | shot_frequency: 10.0 76 | safe_shot_frequency: 10.0 77 | precise_shot_frequency: 4.0 78 | eject_frequency: 5.0 79 | eject_time: 0.2 80 | deep_eject_frequency: 2.0 81 | deep_eject_time: 0.5 82 | single_shot_max_stop_delay: 2.0 83 | 84 | shooting_recorder: 85 | ros__parameters: 86 | friction_wheel_count: 4 87 | log_mode: 2 # 1: trigger, 2: timing 88 | 89 | second_left_friction_velocity_pid_controller: 90 | ros__parameters: 91 | measurement: /gimbal/second_left_friction/velocity 92 | setpoint: /gimbal/second_left_friction/control_velocity 93 | control: /gimbal/second_left_friction/control_torque 94 | kp: 0.003436926 95 | ki: 0.00 96 | kd: 0.009373434 97 | 98 | first_left_friction_velocity_pid_controller: 99 | ros__parameters: 100 | measurement: /gimbal/first_left_friction/velocity 101 | setpoint: /gimbal/first_left_friction/control_velocity 102 | control: /gimbal/first_left_friction/control_torque 103 | kp: 0.003436926 104 | ki: 0.00 105 | kd: 0.009373434 106 | 107 | first_right_friction_velocity_pid_controller: 108 | ros__parameters: 109 | measurement: /gimbal/first_right_friction/velocity 110 | setpoint: /gimbal/first_right_friction/control_velocity 111 | control: /gimbal/first_right_friction/control_torque 112 | kp: 0.003436926 113 | ki: 0.00 114 | kd: 0.009373434 115 | 116 | second_right_friction_velocity_pid_controller: 117 | ros__parameters: 118 | measurement: /gimbal/second_right_friction/velocity 119 | setpoint: /gimbal/second_right_friction/control_velocity 120 | control: /gimbal/second_right_friction/control_torque 121 | kp: 0.003436926 122 | ki: 0.00 123 | kd: 0.009373434 124 | 125 | bullet_feeder_velocity_pid_controller: 126 | ros__parameters: 127 | measurement: /gimbal/bullet_feeder/velocity 128 | setpoint: /gimbal/bullet_feeder/control_velocity 129 | control: /gimbal/bullet_feeder/control_torque 130 | kp: 1.0 131 | ki: 0.001 132 | kd: 0.0 133 | integral_min: -1000.0 134 | integral_max: 1000.0 135 | 136 | left_front_wheel_velocity_pid_controller: 137 | ros__parameters: 138 | measurement: /chassis/left_front_wheel/velocity 139 | setpoint: /chassis/left_front_wheel/control_velocity 140 | control: /chassis/left_front_wheel/control_torque_unrestricted 141 | kp: 0.185 142 | ki: 0.00 143 | kd: 0.00 144 | 145 | left_back_wheel_velocity_pid_controller: 146 | ros__parameters: 147 | measurement: /chassis/left_back_wheel/velocity 148 | setpoint: /chassis/left_back_wheel/control_velocity 149 | control: /chassis/left_back_wheel/control_torque_unrestricted 150 | kp: 0.185 151 | ki: 0.00 152 | kd: 0.00 153 | 154 | right_back_wheel_velocity_pid_controller: 155 | ros__parameters: 156 | measurement: /chassis/right_back_wheel/velocity 157 | setpoint: /chassis/right_back_wheel/control_velocity 158 | control: /chassis/right_back_wheel/control_torque_unrestricted 159 | kp: 0.185 160 | ki: 0.00 161 | kd: 0.00 162 | 163 | right_front_wheel_velocity_pid_controller: 164 | ros__parameters: 165 | measurement: /chassis/right_front_wheel/velocity 166 | setpoint: /chassis/right_front_wheel/control_velocity 167 | control: /chassis/right_front_wheel/control_torque_unrestricted 168 | kp: 0.185 169 | ki: 0.00 170 | kd: 0.00 171 | 172 | auto_aim_controller: 173 | ros__parameters: 174 | # capture 175 | use_video: false # If true, use video stream instead of camera. 176 | video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" 177 | exposure_time: 3 178 | invert_image: false 179 | # identifier 180 | armor_model_path: "/models/mlp.onnx" 181 | # pnp 182 | fx: 1.722231837421459e+03 183 | fy: 1.724876404292754e+03 184 | cx: 7.013056440882832e+02 185 | cy: 5.645821718351237e+02 186 | k1: -0.064232403853946 187 | k2: -0.087667493884102 188 | k3: 0.792381808294582 189 | # tracker 190 | armor_predict_duration: 500 191 | # controller 192 | gimbal_predict_duration: 100 193 | yaw_error: 0.02 194 | pitch_error: 0.06 195 | shoot_velocity: 28.0 196 | predict_sec: 0.095 197 | # etc 198 | buff_predict_duration: 200 199 | buff_model_path: "/models/buff_nocolor_v6.onnx" 200 | omni_exposure: 1000.0 201 | record_fps: 120 202 | debug: false # Setup in actual using.Debug mode is used when referee is not ready 203 | debug_color: 0 # 0 For blue while 1 for red. mine 204 | debug_robot_id: 4 205 | debug_buff_mode: false 206 | record: false 207 | raw_img_pub: false # Set false in actual use 208 | image_viewer_type: 0 209 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/config/infantry_a.yaml: -------------------------------------------------------------------------------- 1 | rmcs_executor: 2 | ros__parameters: 3 | update_rate: 1000.0 4 | components: 5 | - rmcs_core::hardware::TunnelInfantry -> tunnel_omni_hardware 6 | 7 | - rmcs_core::controller::gimbal::GimbalController -> gimbal_controller 8 | 9 | - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller 10 | - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller 11 | - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller 12 | 13 | - rmcs_core::controller::gimbal::ShootingController -> shooting_controller 14 | 15 | - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller 16 | - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller 17 | - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller 18 | 19 | - rmcs_core::controller::chassis::ChassisController -> chassis_controller 20 | 21 | - rmcs_core::controller::chassis::OmniWheelController -> omni_wheel_controller 22 | 23 | - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster 24 | 25 | - rmcs_core::referee::Status -> referee_status 26 | 27 | - rmcs_core::referee::command::Interaction -> referee_interaction 28 | # - rmcs_core::referee::command::interaction::Ui -> referee_ui 29 | # - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry 30 | - rmcs_core::referee::Command -> referee_command 31 | 32 | # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer 33 | # - rmcs_auto_aim::AutoAimController -> auto_aim_controller 34 | 35 | tunnel_omni_hardware: 36 | ros__parameters: 37 | usb_pid: -1 38 | yaw_motor_zero_point: 567 39 | pitch_motor_zero_point: 6416 40 | imu_gx_bias: +0.001294 41 | imu_gy_bias: -0.000584 42 | imu_gz_bias: +0.000445 # Updated at 30th July 21:28 43 | 44 | referee_status: 45 | ros__parameters: 46 | path: /dev/ttyUSB0 47 | 48 | gimbal_controller: 49 | ros__parameters: 50 | upper_limit: -0.4598 51 | lower_limit: 0.4362 52 | 53 | yaw_angle_pid_controller: 54 | ros__parameters: 55 | measurement: /gimbal/yaw/control_angle_error 56 | control: /gimbal/yaw/control_velocity 57 | kp: 15.0 58 | ki: 0.0 59 | kd: 0.0 60 | 61 | yaw_velocity_pid_controller: 62 | ros__parameters: 63 | measurement: /gimbal/yaw/velocity_imu 64 | setpoint: /gimbal/yaw/control_velocity 65 | control: /gimbal/yaw/control_torque 66 | kp: 3.5 67 | ki: 0.0 68 | kd: 0.0 69 | 70 | pitch_angle_pid_controller: 71 | ros__parameters: 72 | measurement: /gimbal/pitch/control_angle_error 73 | control: /gimbal/pitch/control_velocity 74 | kp: 10.00 75 | ki: 0.0 76 | kd: 0.0 77 | 78 | shooting_controller: 79 | ros__parameters: 80 | friction_wheels: 81 | - /gimbal/left_friction 82 | - /gimbal/right_friction 83 | friction_velocities: 84 | - 740.0 85 | - 740.0 86 | is_42mm: false 87 | bullets_per_feeder_turn: 8.0 88 | shot_frequency: 20.0 89 | safe_shot_frequency: 10.0 90 | precise_shot_frequency: 10.0 91 | eject_frequency: 10.0 92 | eject_time: 0.05 93 | deep_eject_frequency: 5.0 94 | deep_eject_time: 0.2 95 | single_shot_max_stop_delay: 2.0 96 | 97 | left_friction_velocity_pid_controller: 98 | ros__parameters: 99 | measurement: /gimbal/left_friction/velocity 100 | setpoint: /gimbal/left_friction/control_velocity 101 | control: /gimbal/left_friction/control_torque 102 | kp: 0.003436926 103 | ki: 0.00 104 | kd: 0.009373434 105 | 106 | right_friction_velocity_pid_controller: 107 | ros__parameters: 108 | measurement: /gimbal/right_friction/velocity 109 | setpoint: /gimbal/right_friction/control_velocity 110 | control: /gimbal/right_friction/control_torque 111 | kp: 0.003436926 112 | ki: 0.00 113 | kd: 0.009373434 114 | 115 | bullet_feeder_velocity_pid_controller: 116 | ros__parameters: 117 | measurement: /gimbal/bullet_feeder/velocity 118 | setpoint: /gimbal/bullet_feeder/control_velocity 119 | control: /gimbal/bullet_feeder/control_torque 120 | kp: 0.583 121 | ki: 0.0 122 | kd: 0.0 123 | 124 | left_front_wheel_velocity_pid_controller: 125 | ros__parameters: 126 | measurement: /chassis/left_front_wheel/velocity 127 | setpoint: /chassis/left_front_wheel/control_velocity 128 | control: /chassis/left_front_wheel/control_torque_unrestricted 129 | kp: 0.185 130 | ki: 0.00 131 | kd: 0.00 132 | 133 | left_back_wheel_velocity_pid_controller: 134 | ros__parameters: 135 | measurement: /chassis/left_back_wheel/velocity 136 | setpoint: /chassis/left_back_wheel/control_velocity 137 | control: /chassis/left_back_wheel/control_torque_unrestricted 138 | kp: 0.185 139 | ki: 0.00 140 | kd: 0.00 141 | 142 | right_back_wheel_velocity_pid_controller: 143 | ros__parameters: 144 | measurement: /chassis/right_back_wheel/velocity 145 | setpoint: /chassis/right_back_wheel/control_velocity 146 | control: /chassis/right_back_wheel/control_torque_unrestricted 147 | kp: 0.185 148 | ki: 0.00 149 | kd: 0.00 150 | 151 | right_front_wheel_velocity_pid_controller: 152 | ros__parameters: 153 | measurement: /chassis/right_front_wheel/velocity 154 | setpoint: /chassis/right_front_wheel/control_velocity 155 | control: /chassis/right_front_wheel/control_torque_unrestricted 156 | kp: 0.185 157 | ki: 0.00 158 | kd: 0.00 159 | 160 | chassis_power_controller: 161 | ros__parameters: 162 | motors: 163 | - /chassis/left_front_wheel 164 | - /chassis/left_back_wheel 165 | - /chassis/right_back_wheel 166 | - /chassis/right_front_wheel 167 | 168 | auto_aim_controller: 169 | ros__parameters: 170 | # capture 171 | use_video: false # If true, use video stream instead of camera. 172 | video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" 173 | exposure_time: 3 174 | invert_image: false 175 | # identifier 176 | armor_model_path: "/models/mlp.onnx" 177 | # pnp 178 | fx: 1.722231837421459e+03 179 | fy: 1.724876404292754e+03 180 | cx: 7.013056440882832e+02 181 | cy: 5.645821718351237e+02 182 | k1: -0.064232403853946 183 | k2: -0.087667493884102 184 | k3: 0.792381808294582 185 | # tracker 186 | armor_predict_duration: 500 187 | # controller 188 | gimbal_predict_duration: 100 189 | yaw_error: 0.02 190 | pitch_error: -0.01 191 | shoot_velocity: 28.0 192 | predict_sec: 0.095 193 | # etc 194 | buff_predict_duration: 200 195 | buff_model_path: "/models/buff_nocolor_v6.onnx" 196 | omni_exposure: 1000.0 197 | record_fps: 120 198 | debug: false # Setup in actual using.Debug mode is used when referee is not ready 199 | debug_color: 0 # 0 For blue while 1 for red. mine 200 | debug_robot_id: 4 201 | debug_buff_mode: false 202 | record: false 203 | raw_img_pub: false # Set false in actual use -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/config/sentry.yaml: -------------------------------------------------------------------------------- 1 | rmcs_executor: 2 | ros__parameters: 3 | update_rate: 1000.0 4 | components: 5 | - rmcs_core::hardware::cboard::Status -> cboard_status 6 | - rmcs_core::referee::Status -> referee_status 7 | 8 | - rmcs_core::controller::gimbal::GimbalController -> gimbal_controller 9 | - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller 10 | - rmcs_core::controller::pid::PidController -> yaw_velocity_pid_controller 11 | - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller 12 | - rmcs_core::controller::pid::PidController -> pitch_velocity_pid_controller 13 | 14 | - rmcs_core::controller::gimbal::ShootingController -> shooting_controller 15 | - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller 16 | - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller 17 | - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller 18 | 19 | - rmcs_core::controller::chassis::ChassisController -> chassis_controller 20 | - rmcs_core::controller::pid::PidController -> left_front_wheel_velocity_pid_controller 21 | - rmcs_core::controller::pid::PidController -> left_back_wheel_velocity_pid_controller 22 | - rmcs_core::controller::pid::PidController -> right_back_wheel_velocity_pid_controller 23 | - rmcs_core::controller::pid::PidController -> right_front_wheel_velocity_pid_controller 24 | - rmcs_core::controller::chassis::PowerController -> chassis_power_controller 25 | 26 | - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster 27 | - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster 28 | 29 | - rmcs_core::referee::command::Interaction -> referee_interaction 30 | - rmcs_core::referee::command::interaction::Ui -> referee_ui 31 | - rmcs_core::referee::app::ui::Infantry -> referee_ui_infantry 32 | 33 | - rmcs_core::hardware::cboard::Command -> cboard_command 34 | - rmcs_core::referee::Command -> referee_command 35 | 36 | - rmcs_core::hardware::cboard::GyroCalibrator -> gyro_calibrator 37 | 38 | - rmcs_auto_aim::Controller -> rmcs_auto_aim 39 | 40 | cboard_status: 41 | ros__parameters: 42 | path: /dev/ttyACM0 43 | yaw_motor_zero_point: 3088 44 | pitch_motor_zero_point: 381 45 | 46 | referee_status: 47 | ros__parameters: 48 | path: /dev/ttyUSB0 49 | 50 | gimbal_controller: 51 | ros__parameters: 52 | upper_limit: -0.4598 53 | lower_limit: 0.4362 54 | 55 | yaw_angle_pid_controller: 56 | ros__parameters: 57 | measurement: /gimbal/yaw/control_angle_error 58 | control: /gimbal/yaw/control_velocity 59 | kp: 15.0 60 | ki: 0.0 61 | kd: 0.0 62 | 63 | yaw_velocity_pid_controller: 64 | ros__parameters: 65 | measurement: /gimbal/yaw/velocity_imu 66 | setpoint: /gimbal/yaw/control_velocity 67 | control: /gimbal/yaw/control_torque 68 | kp: 3.5 69 | ki: 0.0 70 | kd: 0.0 71 | 72 | pitch_angle_pid_controller: 73 | ros__parameters: 74 | measurement: /gimbal/pitch/control_angle_error 75 | control: /gimbal/pitch/control_velocity 76 | kp: 29.64 77 | ki: 0.0 78 | kd: 0.0 79 | 80 | pitch_velocity_pid_controller: 81 | ros__parameters: 82 | measurement: /gimbal/pitch/velocity_imu 83 | setpoint: /gimbal/pitch/control_velocity 84 | control: /gimbal/pitch/control_torque 85 | kp: 1.7 86 | ki: 0.0 87 | kd: 0.0 88 | 89 | shooting_controller: 90 | ros__parameters: 91 | friction_velocity: 770.0 92 | shot_frequency: 20.0 93 | safe_shot_frequency: 10.0 94 | 95 | left_friction_velocity_pid_controller: 96 | ros__parameters: 97 | measurement: /gimbal/left_friction/velocity 98 | setpoint: /gimbal/left_friction/control_velocity 99 | control: /gimbal/left_friction/control_torque 100 | kp: 0.003436926 101 | ki: 0.00 102 | kd: 0.009373434 103 | 104 | right_friction_velocity_pid_controller: 105 | ros__parameters: 106 | measurement: /gimbal/right_friction/velocity 107 | setpoint: /gimbal/right_friction/control_velocity 108 | control: /gimbal/right_friction/control_torque 109 | kp: 0.003436926 110 | ki: 0.00 111 | kd: 0.009373434 112 | 113 | bullet_feeder_velocity_pid_controller: 114 | ros__parameters: 115 | measurement: /gimbal/bullet_feeder/velocity 116 | setpoint: /gimbal/bullet_feeder/control_velocity 117 | control: /gimbal/bullet_feeder/control_torque 118 | kp: 0.583 119 | ki: 0.0 120 | kd: 0.0 121 | 122 | left_front_wheel_velocity_pid_controller: 123 | ros__parameters: 124 | measurement: /chassis/left_front_wheel/velocity 125 | setpoint: /chassis/left_front_wheel/control_velocity 126 | control: /chassis/left_front_wheel/control_torque_unrestricted 127 | kp: 0.185 128 | ki: 0.00 129 | kd: 0.00 130 | 131 | left_back_wheel_velocity_pid_controller: 132 | ros__parameters: 133 | measurement: /chassis/left_back_wheel/velocity 134 | setpoint: /chassis/left_back_wheel/control_velocity 135 | control: /chassis/left_back_wheel/control_torque_unrestricted 136 | kp: 0.185 137 | ki: 0.00 138 | kd: 0.00 139 | 140 | right_back_wheel_velocity_pid_controller: 141 | ros__parameters: 142 | measurement: /chassis/right_back_wheel/velocity 143 | setpoint: /chassis/right_back_wheel/control_velocity 144 | control: /chassis/right_back_wheel/control_torque_unrestricted 145 | kp: 0.185 146 | ki: 0.00 147 | kd: 0.00 148 | 149 | right_front_wheel_velocity_pid_controller: 150 | ros__parameters: 151 | measurement: /chassis/right_front_wheel/velocity 152 | setpoint: /chassis/right_front_wheel/control_velocity 153 | control: /chassis/right_front_wheel/control_torque_unrestricted 154 | kp: 0.185 155 | ki: 0.00 156 | kd: 0.00 157 | 158 | chassis_power_controller: 159 | ros__parameters: 160 | motors: 161 | - /chassis/left_front_wheel 162 | - /chassis/left_back_wheel 163 | - /chassis/right_back_wheel 164 | - /chassis/right_front_wheel 165 | 166 | rmcs_auto_aim: 167 | ros__parameters: 168 | exposure_time: 5 169 | armor_predict_duration: 500 170 | buff_predict_duration: 200 171 | gimbal_predict_duration: 500 172 | yaw_error: 0.005 173 | pitch_error: -0.5 174 | armor_model_path: "/models/armoridentify_with_base.onnx" 175 | buff_model_path: "/models/buff_nocolor_v6.onnx" 176 | fx: 1.722231837421459e+03 177 | fy: 1.724876404292754e+03 178 | cx: 7.013056440882832e+02 179 | cy: 5.645821718351237e+02 180 | k1: -0.064232403853946 181 | k2: -0.087667493884102 182 | k3: 0.792381808294582 183 | omni_exposure: 1000.0 184 | test: 0.1107 185 | omni_fx: 1.0941e+03 186 | omni_fy: 1.0961e+03 187 | omni_cx: 9.596707e+02 188 | omni_cy: 5.860586e+02 189 | omni_k1: 0.0829 190 | omni_k2: -0.1184 191 | omni_k3: 0.0501 192 | test: 0.1107 193 | record_fps: 165 194 | debug: true 195 | debug_color: 0 # 0 For blue while 1 for red 196 | debug_robot_id: 7 197 | debug_buff_mode: false 198 | record: false 199 | raw_img_pub: false -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py: -------------------------------------------------------------------------------- 1 | from typing import List, Optional 2 | import os 3 | 4 | from launch import ( 5 | LaunchContext, 6 | LaunchDescription, 7 | LaunchDescriptionEntity, 8 | ) 9 | from launch.actions import LogInfo 10 | from launch.substitutions import LaunchConfiguration 11 | 12 | from launch_ros.actions import Node 13 | from launch_ros.substitutions import FindPackageShare 14 | 15 | 16 | class MyLaunchDescriptionEntity(LaunchDescriptionEntity): 17 | def visit( 18 | self, context: "LaunchContext" 19 | ) -> Optional[List["LaunchDescriptionEntity"]]: 20 | entities = [] 21 | 22 | robot_config = LaunchConfiguration("robot").perform(context) 23 | if robot_config.startswith("auto."): 24 | is_automatic = True 25 | robot_name = robot_config[5:] 26 | else: 27 | is_automatic = False 28 | robot_name = robot_config 29 | 30 | entities.append( 31 | LogInfo( 32 | msg=f"Starting RMCS on robot '{robot_config}'{'(automatic)' if is_automatic else ''} -> {robot_name}.yaml" 33 | ) 34 | ) 35 | 36 | entities.append( 37 | Node( 38 | package="rmcs_executor", 39 | executable="rmcs_executor", 40 | parameters=[ 41 | os.path.join( 42 | FindPackageShare("rmcs_bringup").perform(context), 43 | "config", 44 | robot_name + ".yaml", 45 | ), 46 | ], 47 | respawn=True, 48 | respawn_delay=1.0, 49 | output="screen", 50 | ) 51 | ) 52 | 53 | if is_automatic: 54 | pass 55 | 56 | return entities 57 | 58 | 59 | def generate_launch_description(): 60 | ld = LaunchDescription([MyLaunchDescriptionEntity()]) 61 | 62 | return ld 63 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | rmcs_bringup 6 | 0.0.0 7 | bringup 8 | Qzh 9 | GPL-3.0-only 10 | 11 | ament_cmake 12 | 13 | joint_state_broadcaster 14 | robot_state_publisher 15 | rviz2 16 | xacro 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(rmcs_core) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-O2 -Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package (ament_cmake_auto REQUIRED) 14 | ament_auto_find_build_dependencies () 15 | 16 | file (GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS 17 | ${PROJECT_SOURCE_DIR}/src/*.cpp 18 | ${PROJECT_SOURCE_DIR}/src/*.c 19 | ) 20 | 21 | ament_auto_add_library ( 22 | ${PROJECT_NAME} SHARED 23 | ${PROJECT_SOURCE} 24 | ) 25 | 26 | include_directories(${PROJECT_SOURCE_DIR}/include) 27 | include_directories(${PROJECT_SOURCE_DIR}/src) 28 | include_directories(${PROJECT_SOURCE_DIR}/librmcs) 29 | 30 | include_directories(SYSTEM "/usr/include/libusb-1.0") 31 | 32 | target_link_libraries(${PROJECT_NAME} -lusb-1.0) 33 | 34 | pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) 35 | 36 | ament_auto_package() -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/include/rmcs_core/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_core/include/rmcs_core/.gitkeep -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcs_core 5 | 0.0.0 6 | TODO: Package description 7 | developer 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | std_msgs 14 | pluginlib 15 | tf2 16 | tf2_ros 17 | serial 18 | rmcs_utility 19 | rmcs_msgs 20 | rmcs_executor 21 | rmcs_description 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Test plugin. 4 | 5 | 6 | Test plugin. 7 | 8 | 9 | Test plugin. 10 | 11 | 12 | Test plugin. 13 | 14 | 15 | Test plugin. 16 | 17 | 18 | Test plugin. 19 | 20 | 21 | Test plugin. 22 | 23 | 24 | Test plugin. 25 | 26 | 27 | Test plugin. 28 | 29 | 30 | the recorder of Hero 31 | 32 | 33 | Test plugin. 34 | 35 | 36 | Test plugin. 37 | 38 | 39 | Test plugin. 40 | 41 | 42 | Test plugin. 43 | 44 | 45 | Test plugin. 46 | 47 | 48 | Test plugin. 49 | 50 | 51 | Test plugin. 52 | 53 | 54 | Test plugin. 55 | 56 | 57 | Test plugin. 58 | 59 | 60 | Test plugin. 61 | 62 | 63 | Test plugin. 64 | 65 | 66 | Test plugin. 67 | 68 | 69 | Test plugin. 70 | 71 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/broadcaster/tf_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rmcs_core::broadcaster { 13 | 14 | class TfBroadcaster 15 | : public rmcs_executor::Component 16 | , public rclcpp::Node { 17 | public: 18 | TfBroadcaster() 19 | : Node{ 20 | get_component_name(), 21 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { 22 | register_input("/predefined/timestamp", timestamp_); 23 | register_input("/predefined/update_count", update_count_); 24 | 25 | register_input("/tf", tf_); 26 | } 27 | ~TfBroadcaster() = default; 28 | 29 | void update() override { 30 | using namespace std::chrono_literals; 31 | if (*update_count_ == 0) 32 | next_publish_timestamp_ = *timestamp_; 33 | if (*timestamp_ >= next_publish_timestamp_) { 34 | fast_tf::rcl::broadcast_all(*tf_); 35 | next_publish_timestamp_ += 50ms; 36 | } 37 | } 38 | 39 | private: 40 | InputInterface update_count_; 41 | InputInterface timestamp_; 42 | std::chrono::steady_clock::time_point next_publish_timestamp_; 43 | 44 | InputInterface tf_; 45 | }; 46 | 47 | } // namespace rmcs_core::broadcaster 48 | 49 | #include 50 | 51 | PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::TfBroadcaster, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rmcs_core::broadcaster { 8 | 9 | class ValueBroadcaster 10 | : public rmcs_executor::Component 11 | , public rclcpp::Node { 12 | public: 13 | ValueBroadcaster() 14 | : Node{get_component_name()} { 15 | declare_parameter>("forward_list", std::vector{}); 16 | parameter_subscriber_ = std::make_unique(this); 17 | parameter_callback_ = parameter_subscriber_->add_parameter_callback( 18 | "forward_list", 19 | [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); 20 | } 21 | 22 | void before_pairing( 23 | const std::map& output_map) override { 24 | for (const auto& [name, type] : output_map) { 25 | if (type == typeid(double)) { 26 | forward_units_.emplace( 27 | name, 28 | std::make_unique>(this, name)); 29 | } 30 | } 31 | std::vector forward_list; 32 | if (get_parameter("forward_list", forward_list)) 33 | update_forward_list(forward_list); 34 | } 35 | 36 | void update() override { 37 | for (auto& [name, unit] : forward_units_) 38 | unit->update(); 39 | } 40 | 41 | private: 42 | void update_forward_list(const std::vector& forward_list) { 43 | for (auto& [name, unit] : forward_units_) 44 | unit->active = false; 45 | 46 | for (auto& name : forward_list) { 47 | auto iter = forward_units_.find(name); 48 | if (iter == forward_units_.end()) { 49 | RCLCPP_ERROR( 50 | get_logger(), 51 | "Unable to find corresponding output of '%s', maybe the type of output is " 52 | "unsupported or the output does not exist.", 53 | name.c_str()); 54 | } else { 55 | auto& unit = iter->second; 56 | unit->active = true; 57 | } 58 | } 59 | 60 | for (auto& [name, unit] : forward_units_) { 61 | if (unit->active) 62 | unit->activate(this); 63 | else 64 | unit->deactivate(); 65 | } 66 | } 67 | 68 | class BasicForwarderUnit { 69 | public: 70 | virtual ~BasicForwarderUnit() {} 71 | 72 | virtual void activate(rclcpp::Node* node) = 0; 73 | virtual void update() = 0; 74 | virtual void deactivate() = 0; 75 | 76 | bool active; 77 | }; 78 | 79 | template 80 | // using StdT = double; 81 | // using RosT = std_msgs::msg::Float64; 82 | class ForwardUnit : public BasicForwarderUnit { 83 | public: 84 | ForwardUnit(Component* component, const std::string& name) 85 | : name_(name) { 86 | component->register_input(name, input_); 87 | } 88 | 89 | void activate(rclcpp::Node* node) override { 90 | if (!publisher_) 91 | publisher_ = node->create_publisher(name_, rclcpp::QoS{5}.reliable()); 92 | } 93 | 94 | void update() override { 95 | if (!publisher_) 96 | return; 97 | 98 | RosT msg; 99 | msg.data = *input_; 100 | publisher_->publish(msg); 101 | } 102 | 103 | void deactivate() override { publisher_ = nullptr; } 104 | 105 | private: 106 | std::string name_; 107 | InputInterface input_; 108 | rclcpp::Publisher::SharedPtr publisher_; 109 | }; 110 | 111 | std::map> forward_units_; 112 | 113 | std::unique_ptr parameter_subscriber_; 114 | std::shared_ptr parameter_callback_; 115 | }; 116 | 117 | } // namespace rmcs_core::broadcaster 118 | 119 | #include 120 | 121 | PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/controller/gimbal/shooting_recorder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | namespace rmcs_core::controller::gimbal { 7 | 8 | class ShootingRecorder 9 | : public rmcs_executor::Component 10 | , public rclcpp::Node { 11 | public: 12 | explicit ShootingRecorder( 13 | const rclcpp::NodeOptions& option = 14 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) 15 | : Node(get_component_name(), option) { 16 | 17 | friction_wheel_count_ = get_parameter("friction_wheel_count").as_int(); 18 | 19 | log_mode_ = static_cast(get_parameter("log_mode").as_int()); 20 | 21 | register_input("/referee/shooter/initial_speed", initial_speed_); 22 | register_input("/referee/shooter/shoot_timestamp", shoot_timestamp_); 23 | register_input("/friction_wheels/temperature", fractional_temperature_); 24 | 25 | if (friction_wheel_count_ == 2) { 26 | const auto topic = std::array{ 27 | "/gimbal/left_friction/control_velocity", 28 | "/gimbal/right_friction/control_velocity", 29 | }; 30 | for (int i = 0; i < 2; i++) 31 | register_input(topic[i], friction_wheels_velocity_[i]); 32 | } else if (friction_wheel_count_ == 4) { 33 | const auto topic = std::array{ 34 | "/gimbal/first_left_friction/control_velocity", 35 | "/gimbal/first_right_friction/control_velocity", 36 | "/gimbal/second_left_friction/control_velocity", 37 | "/gimbal/second_right_friction/control_velocity", 38 | }; 39 | for (int i = 0; i < 4; i++) 40 | register_input(topic[i], friction_wheels_velocity_[i]); 41 | } 42 | 43 | using namespace std::chrono; 44 | auto now = high_resolution_clock::now(); 45 | auto ms = duration_cast(now.time_since_epoch()).count(); 46 | 47 | auto file = fmt::format("/robot_shoot/{}.log", ms); 48 | log_stream_.open(file); 49 | } 50 | 51 | ~ShootingRecorder() { log_stream_.close(); } 52 | 53 | void update() override { 54 | 55 | switch (log_mode_) { 56 | case LogMode::TRIGGER: 57 | // It will be triggered by shooting action 58 | if (*shoot_timestamp_ == last_shoot_timestamp_) 59 | return; 60 | break; 61 | case LogMode::TIMING: 62 | // 10Hz to log 63 | if (log_count_++ % 100) 64 | return; 65 | break; 66 | } 67 | 68 | auto log_text = std::string{}; 69 | auto timestamp = timestamp_to_string(*shoot_timestamp_); 70 | 71 | if (friction_wheel_count_ == 2) { 72 | log_text = fmt::format( 73 | "{},{},{},{},{}", timestamp, *initial_speed_, // 74 | *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], 75 | *fractional_temperature_); 76 | } else if (friction_wheel_count_ == 4) { 77 | log_text = fmt::format( 78 | "{},{},{},{},{},{},{}", timestamp, *initial_speed_, // 79 | *friction_wheels_velocity_[0], *friction_wheels_velocity_[1], 80 | *friction_wheels_velocity_[2], *friction_wheels_velocity_[3], 81 | *fractional_temperature_); 82 | } 83 | 84 | log_stream_ << log_text << std::endl; 85 | RCLCPP_INFO(get_logger(), "%s", log_text.c_str()); 86 | 87 | last_shoot_timestamp_ = *shoot_timestamp_; 88 | } 89 | 90 | private: 91 | /// @brief Component interface 92 | InputInterface initial_speed_; 93 | InputInterface shoot_timestamp_; 94 | 95 | InputInterface fractional_temperature_; 96 | 97 | std::size_t friction_wheel_count_ = 2; 98 | std::array, 4> friction_wheels_velocity_; 99 | 100 | /// @brief For log 101 | enum class LogMode { TRIGGER = 1, TIMING = 2 }; 102 | LogMode log_mode_ = LogMode::TRIGGER; 103 | 104 | double last_shoot_timestamp_ = 0; 105 | std::ofstream log_stream_; 106 | 107 | std::size_t log_count_ = 0; 108 | 109 | private: 110 | static std::string timestamp_to_string(double timestamp) { 111 | auto time = static_cast(timestamp); 112 | auto local_time = std::localtime(&time); 113 | 114 | char buffer[100]; 115 | std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); 116 | 117 | double fractional_seconds = timestamp - std::floor(timestamp); 118 | int milliseconds = static_cast(fractional_seconds * 1000); 119 | 120 | char result[150]; 121 | std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); 122 | 123 | return result; 124 | } 125 | }; 126 | 127 | } // namespace rmcs_core::controller::gimbal 128 | 129 | #include 130 | 131 | PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::ShootingRecorder, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "pid_calculator.hpp" 7 | 8 | namespace rmcs_core::controller::pid { 9 | 10 | class ErrorPidController 11 | : public rmcs_executor::Component 12 | , public rclcpp::Node { 13 | public: 14 | ErrorPidController() 15 | : Node( 16 | get_component_name(), 17 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) 18 | , pid_calculator_( 19 | get_parameter("kp").as_double(), get_parameter("ki").as_double(), 20 | get_parameter("kd").as_double()) { 21 | 22 | register_input(get_parameter("measurement").as_string(), measurement_); 23 | register_output(get_parameter("control").as_string(), control_); 24 | 25 | get_parameter("integral_min", pid_calculator_.integral_min); 26 | get_parameter("integral_max", pid_calculator_.integral_max); 27 | get_parameter("output_min", pid_calculator_.output_min); 28 | get_parameter("output_max", pid_calculator_.output_max); 29 | } 30 | 31 | void update() override { 32 | auto err = *measurement_; 33 | *control_ = pid_calculator_.update(err); 34 | } 35 | 36 | private: 37 | PidCalculator pid_calculator_; 38 | 39 | InputInterface measurement_; 40 | 41 | OutputInterface control_; 42 | }; 43 | 44 | } // namespace rmcs_core::controller::pid 45 | 46 | #include 47 | 48 | PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace rmcs_core::controller::pid { 11 | 12 | template 13 | class MatrixPidCalculator { 14 | using Vector = Eigen::Vector; 15 | using Gain = std::conditional, double>::type; 16 | 17 | public: 18 | MatrixPidCalculator(Gain kp, Gain ki, Gain kd) 19 | : kp(std::move(kp)) 20 | , ki(std::move(ki)) 21 | , kd(std::move(kd)) { 22 | integral_min.setConstant(-inf); 23 | integral_max.setConstant(inf); 24 | output_min.setConstant(-inf); 25 | output_max.setConstant(inf); 26 | reset(); 27 | } 28 | 29 | virtual ~MatrixPidCalculator() = default; 30 | 31 | void reset() { 32 | last_err_.setConstant(nan); 33 | err_integral_ = Vector::Zero(); 34 | } 35 | 36 | Vector update(Vector err) { 37 | if (!std::isfinite(err[0])) { 38 | Vector nan_vector; 39 | nan_vector.setConstant(nan); 40 | return nan_vector; 41 | } else { 42 | Vector control = kp * err + ki * err_integral_; 43 | err_integral_ = clamp(err_integral_ + err, integral_min, integral_max); 44 | 45 | if (!std::isnan(last_err_[0])) 46 | control += kd * (err - last_err_); 47 | last_err_ = err; 48 | 49 | return clamp(control, output_min, output_max); 50 | } 51 | } 52 | 53 | Gain kp, ki, kd; 54 | Vector integral_min, integral_max; 55 | Vector output_min, output_max; 56 | 57 | private: 58 | static constexpr double inf = std::numeric_limits::infinity(); 59 | static constexpr double nan = std::numeric_limits::quiet_NaN(); 60 | 61 | static Vector clamp(const Vector& value, const Vector& min, const Vector& max) { 62 | return value.cwiseMax(min).cwiseMin(max); 63 | } 64 | 65 | Vector last_err_, err_integral_; 66 | }; 67 | 68 | } // namespace rmcs_core::controller::pid -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace rmcs_core::controller::pid { 9 | 10 | class PidCalculator { 11 | public: 12 | PidCalculator(double kp, double ki, double kd) 13 | : kp(kp) 14 | , ki(ki) 15 | , kd(kd) { 16 | reset(); 17 | } 18 | 19 | virtual ~PidCalculator() = default; 20 | 21 | void reset() { 22 | last_err_ = nan; 23 | err_integral_ = 0; 24 | } 25 | 26 | double update(double err) { 27 | if (!std::isfinite(err)) { 28 | return nan; 29 | } else { 30 | double control = kp * err + ki * err_integral_; 31 | err_integral_ = std::clamp(err_integral_ + err, integral_min, integral_max); 32 | 33 | if (!std::isnan(last_err_)) 34 | control += kd * (err - last_err_); 35 | last_err_ = err; 36 | 37 | return std::clamp(control, output_min, output_max); 38 | } 39 | } 40 | 41 | double kp, ki, kd; 42 | double integral_min = -inf, integral_max = inf; 43 | double output_min = -inf, output_max = inf; 44 | 45 | private: 46 | static constexpr double inf = std::numeric_limits::infinity(); 47 | static constexpr double nan = std::numeric_limits::quiet_NaN(); 48 | 49 | double last_err_, err_integral_; 50 | }; 51 | 52 | } // namespace rmcs_core::controller::pid -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "pid_calculator.hpp" 7 | 8 | namespace rmcs_core::controller::pid { 9 | 10 | class PidController 11 | : public rmcs_executor::Component 12 | , public rclcpp::Node { 13 | public: 14 | PidController() 15 | : Node( 16 | get_component_name(), 17 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) 18 | , pid_calculator_( 19 | get_parameter("kp").as_double(), get_parameter("ki").as_double(), 20 | get_parameter("kd").as_double()) { 21 | 22 | register_input(get_parameter("measurement").as_string(), measurement_); 23 | 24 | // Allows using immediate value instead of message name 25 | auto parameter_setpoint = get_parameter("setpoint"); 26 | if (parameter_setpoint.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { 27 | setpoint_.make_and_bind_directly(parameter_setpoint.as_double()); 28 | } else { 29 | register_input(parameter_setpoint.as_string(), setpoint_); 30 | } 31 | 32 | register_output(get_parameter("control").as_string(), control_); 33 | 34 | get_parameter("integral_min", pid_calculator_.integral_min); 35 | get_parameter("integral_max", pid_calculator_.integral_max); 36 | get_parameter("output_min", pid_calculator_.output_min); 37 | get_parameter("output_max", pid_calculator_.output_max); 38 | } 39 | 40 | void update() override { 41 | auto err = *setpoint_ - *measurement_; 42 | *control_ = pid_calculator_.update(err); 43 | } 44 | 45 | private: 46 | PidCalculator pid_calculator_; 47 | 48 | InputInterface measurement_; 49 | InputInterface setpoint_; 50 | 51 | OutputInterface control_; 52 | }; 53 | 54 | } // namespace rmcs_core::controller::pid 55 | 56 | #include 57 | 58 | PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::PidController, rmcs_executor::Component) 59 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/benewake.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace rmcs_core::hardware::device { 13 | using rmcs_executor::Component; 14 | 15 | class Benewake { 16 | public: 17 | explicit Benewake(Component& status_component, const std::string& name) { 18 | status_component.register_output(name, distance_, nan_); 19 | } 20 | 21 | void store_status(const std::byte* uart_data, size_t uart_data_length) { 22 | if (uart_data_length != sizeof(Package)) { 23 | return; 24 | } 25 | 26 | Package package; 27 | uint8_t checksum; 28 | std::memcpy(&package, uart_data, sizeof(Package)); 29 | std::memcpy(&checksum, uart_data + sizeof(Package), sizeof(uint8_t)); 30 | 31 | if (checksum != package.calculate_checksum()) { 32 | return; 33 | } 34 | 35 | if (package.header[0] != 0x59 || package.header[1] != 0x59) { 36 | return; 37 | } 38 | package_.store(package, std::memory_order::relaxed); 39 | } 40 | 41 | void update_status() { 42 | const auto package = package_.load(std::memory_order::relaxed); 43 | 44 | *distance_ = package.calculate_distance(); 45 | signal_strength_ = package.calculate_signal_strength(); 46 | } 47 | 48 | double get_distance() const { return *distance_; } 49 | double get_signal_strength() const { return signal_strength_; } 50 | 51 | private: 52 | static constexpr double nan_ = std::numeric_limits::quiet_NaN(); 53 | 54 | struct __attribute__((packed)) Package { 55 | uint8_t header[2]; 56 | uint8_t distance[2]; 57 | uint8_t signal_strength[2]; 58 | uint8_t reserved[2]; 59 | 60 | double calculate_distance() const { 61 | return static_cast(distance[0] * 256 + distance[1]); 62 | } 63 | 64 | double calculate_signal_strength() const { 65 | return static_cast(signal_strength[0] * 256 + signal_strength[1]); 66 | } 67 | 68 | double calculate_reserved() const { 69 | return static_cast(reserved[0] * 256 + reserved[1]); 70 | } 71 | 72 | uint8_t calculate_checksum() const { 73 | return header[0] ^ header[1] ^ distance[0] ^ distance[1] ^ signal_strength[0] 74 | ^ signal_strength[1] ^ reserved[0] ^ reserved[1]; 75 | } 76 | }; 77 | 78 | std::atomic package_; 79 | static_assert(decltype(package_)::is_always_lock_free); 80 | 81 | Component::OutputInterface distance_; 82 | double signal_strength_ = nan_; 83 | }; 84 | 85 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/bmi088.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_core::hardware::device { 6 | 7 | using Bmi088 = librmcs::device::Bmi088; 8 | 9 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_core::hardware::device { 7 | 8 | class DjiMotor : public librmcs::device::DjiMotor { 9 | public: 10 | DjiMotor( 11 | rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, 12 | const std::string& name_prefix) 13 | : librmcs::device::DjiMotor() { 14 | status_component.register_output(name_prefix + "/angle", angle_, 0.0); 15 | status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); 16 | status_component.register_output(name_prefix + "/torque", torque_, 0.0); 17 | status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); 18 | 19 | command_component.register_input(name_prefix + "/control_torque", control_torque_, false); 20 | } 21 | 22 | DjiMotor( 23 | rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, 24 | const std::string& name_prefix, const Config& config) 25 | : DjiMotor(status_component, command_component, name_prefix) { 26 | configure(config); 27 | } 28 | 29 | void configure(const Config& config) { 30 | librmcs::device::DjiMotor::configure(config); 31 | 32 | *max_torque_ = max_torque(); 33 | } 34 | 35 | void update_status() { 36 | librmcs::device::DjiMotor::update_status(); 37 | *angle_ = angle(); 38 | *velocity_ = velocity(); 39 | *torque_ = torque(); 40 | } 41 | 42 | double control_torque() const { 43 | if (control_torque_.ready()) [[likely]] 44 | return *control_torque_; 45 | else 46 | return 0.0; 47 | } 48 | 49 | uint16_t generate_command() { 50 | return librmcs::device::DjiMotor::generate_command(control_torque()); 51 | } 52 | 53 | private: 54 | rmcs_executor::Component::OutputInterface angle_; 55 | rmcs_executor::Component::OutputInterface velocity_; 56 | rmcs_executor::Component::OutputInterface torque_; 57 | rmcs_executor::Component::OutputInterface max_torque_; 58 | 59 | rmcs_executor::Component::InputInterface control_torque_; 60 | }; 61 | 62 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace rmcs_core::hardware::device { 11 | 12 | class Dr16 : public librmcs::device::Dr16 { 13 | public: 14 | explicit Dr16(rmcs_executor::Component& component) { 15 | component.register_output( 16 | "/remote/joystick/right", joystick_right_, Eigen::Vector2d::Zero()); 17 | component.register_output("/remote/joystick/left", joystick_left_, Eigen::Vector2d::Zero()); 18 | 19 | component.register_output( 20 | "/remote/switch/right", switch_right_, rmcs_msgs::Switch::UNKNOWN); 21 | component.register_output("/remote/switch/left", switch_left_, rmcs_msgs::Switch::UNKNOWN); 22 | 23 | component.register_output( 24 | "/remote/mouse/velocity", mouse_velocity_, Eigen::Vector2d::Zero()); 25 | 26 | component.register_output("/remote/mouse", mouse_); 27 | std::memset(&*mouse_, 0, sizeof(*mouse_)); 28 | component.register_output("/remote/keyboard", keyboard_); 29 | std::memset(&*keyboard_, 0, sizeof(*keyboard_)); 30 | 31 | component.register_output("/remote/rotary_knob", rotary_knob_); 32 | } 33 | 34 | void update_status() { 35 | librmcs::device::Dr16::update_status(); 36 | 37 | *joystick_right_ = joystick_right(); 38 | *joystick_left_ = joystick_left(); 39 | 40 | *switch_right_ = switch_right(); 41 | *switch_left_ = switch_left(); 42 | 43 | *mouse_velocity_ = mouse_velocity(); 44 | 45 | *mouse_ = mouse(); 46 | *keyboard_ = keyboard(); 47 | 48 | *rotary_knob_ = rotary_knob(); 49 | } 50 | 51 | Eigen::Vector2d joystick_right() const { 52 | return to_eigen_vector(librmcs::device::Dr16::joystick_right()); 53 | } 54 | Eigen::Vector2d joystick_left() const { 55 | return to_eigen_vector(librmcs::device::Dr16::joystick_left()); 56 | } 57 | 58 | rmcs_msgs::Switch switch_right() const { 59 | return std::bit_cast(librmcs::device::Dr16::switch_right()); 60 | } 61 | rmcs_msgs::Switch switch_left() const { 62 | return std::bit_cast(librmcs::device::Dr16::switch_left()); 63 | } 64 | 65 | Eigen::Vector2d mouse_velocity() const { 66 | return to_eigen_vector(librmcs::device::Dr16::mouse_velocity()); 67 | } 68 | 69 | rmcs_msgs::Mouse mouse() const { 70 | return std::bit_cast(librmcs::device::Dr16::mouse()); 71 | } 72 | rmcs_msgs::Keyboard keyboard() const { 73 | return std::bit_cast(librmcs::device::Dr16::keyboard()); 74 | } 75 | 76 | private: 77 | static Eigen::Vector2d to_eigen_vector(Vector vector) { return {vector.x, vector.y}; } 78 | 79 | rmcs_executor::Component::OutputInterface joystick_right_; 80 | rmcs_executor::Component::OutputInterface joystick_left_; 81 | 82 | rmcs_executor::Component::OutputInterface switch_right_; 83 | rmcs_executor::Component::OutputInterface switch_left_; 84 | 85 | rmcs_executor::Component::OutputInterface mouse_velocity_; 86 | 87 | rmcs_executor::Component::OutputInterface mouse_; 88 | rmcs_executor::Component::OutputInterface keyboard_; 89 | 90 | rmcs_executor::Component::OutputInterface rotary_knob_; 91 | }; 92 | 93 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/gy614.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace rmcs_core::hardware::device { 13 | using rmcs_executor::Component; 14 | 15 | /// @brief GY-614V3BAA, a kind of temperature sensor 16 | class Gy614 { 17 | public: 18 | explicit Gy614(Component& status_component, const std::string& name) { 19 | status_component.register_output( 20 | name, target_temperature_, std::numeric_limits::quiet_NaN()); 21 | }; 22 | 23 | void store_status(const std::byte* uart_data, size_t uart_data_length) { 24 | if (uart_data_length != package_length) 25 | return; 26 | 27 | // TODO: CRC check 28 | 29 | Package package; 30 | std::memcpy(&package, uart_data + 4, sizeof(Package)); 31 | package_.store(package, std::memory_order::relaxed); 32 | } 33 | 34 | void update_status() { 35 | const auto package = package_.load(std::memory_order::relaxed); 36 | 37 | emissivity_ = package.calculate_emissivity(); 38 | *target_temperature_ = package.calculate_target_temperature(); 39 | environment_temperature_ = package.calculate_environment_temperature(); 40 | body_temperature_from_forhead_ = package.calculate_body_temperature_from_forhead(); 41 | } 42 | 43 | double emissivity() const { return emissivity_; } 44 | double target_temperature() const { return *target_temperature_; } 45 | double environment_temperature() const { return environment_temperature_; } 46 | double body_temperature_from_forhead() const { return body_temperature_from_forhead_; } 47 | 48 | static constexpr size_t package_length = 12; 49 | 50 | private: 51 | struct __attribute__((packed)) Package { 52 | uint8_t e; 53 | uint8_t to[2]; 54 | uint8_t ta[2]; 55 | uint8_t bo[2]; 56 | uint8_t crc; 57 | 58 | double calculate_emissivity() const { // 59 | return static_cast(e) / 100; 60 | } 61 | double calculate_target_temperature() const { 62 | return static_cast(to[0] * 256 + to[1]) / 100; 63 | } 64 | double calculate_environment_temperature() const { 65 | return static_cast(ta[0] * 256 + ta[1]) / 100; 66 | } 67 | double calculate_body_temperature_from_forhead() const { 68 | return static_cast(bo[0] * 256 + bo[1]) / 100; 69 | } 70 | }; 71 | 72 | std::atomic package_; 73 | static_assert(decltype(package_)::is_always_lock_free); 74 | 75 | double emissivity_ = 0; 76 | Component::OutputInterface target_temperature_; 77 | double environment_temperature_ = 0; 78 | double body_temperature_from_forhead_ = 0; 79 | }; 80 | 81 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/hipnuc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rmcs_core::hardware::device { 13 | 14 | class Hipnuc { 15 | public: 16 | explicit Hipnuc() = default; 17 | 18 | bool store_status(const std::byte* uart_data, size_t uart_data_length) { 19 | struct { 20 | const std::byte* data; 21 | size_t data_length; 22 | 23 | size_t read(std::byte* buffer, size_t size) { 24 | if (size < data_length) 25 | size = data_length; 26 | 27 | std::memcpy(buffer, data, size); 28 | data += size; 29 | data_length -= size; 30 | 31 | return size; 32 | }; 33 | } stream{uart_data, uart_data_length}; 34 | 35 | bool success = false; 36 | while (stream.data_length) 37 | success |= store_status(stream); 38 | return success; 39 | } 40 | 41 | template 42 | bool store_status(rmcs_utility::is_readable_stream auto& stream) { 43 | auto result = rmcs_utility::receive_package( 44 | stream, package_, cache_size_, uint16_t{0xA55A}, [](const auto& package) { 45 | uint16_t crc = 0; 46 | crc16_update( 47 | &crc, reinterpret_cast(&package.header), 48 | sizeof(package.header) + sizeof(package.length)); 49 | crc16_update( 50 | &crc, reinterpret_cast(&package.data_label), 51 | sizeof(package.data_label) + sizeof(package.q)); 52 | return crc == package.crc; 53 | }); 54 | if (result == rmcs_utility::ReceiveResult::SUCCESS) { 55 | cache_size_ = 0; 56 | quaternion_buffer_.write(package_.q); 57 | return true; 58 | } else if (result == rmcs_utility::ReceiveResult::HEADER_INVALID) 59 | RCLCPP_ERROR(rclcpp::get_logger("ch040_imu"), "Header invalid"); 60 | else if (result == rmcs_utility::ReceiveResult::VERIFY_INVALID) 61 | RCLCPP_ERROR(rclcpp::get_logger("ch040_imu"), "Data CRC invalid"); 62 | return false; 63 | } 64 | 65 | void update_status() { 66 | QuaternionData quaternion; 67 | if (quaternion_buffer_.read(quaternion)) 68 | quaternion_ = 69 | Eigen::Quaterniond{quaternion.w, quaternion.x, quaternion.y, quaternion.z}; 70 | } 71 | 72 | const Eigen::Quaterniond& quaternion() const { return quaternion_; } 73 | 74 | private: 75 | struct __attribute__((packed)) QuaternionData { 76 | float w; 77 | float x; 78 | float y; 79 | float z; 80 | }; 81 | struct __attribute__((packed)) Package { 82 | uint16_t header; // 0xA55A 83 | uint16_t length; // 17 = sizeof(data_label) + sizeof(QuaternionData) 84 | uint16_t crc; // Checksum of all fields except the CRC itself 85 | uint8_t data_label; // 0xD1 = Quaternion 86 | QuaternionData q; 87 | } package_; 88 | size_t cache_size_ = 0; 89 | 90 | rmcs_utility::DoubleBuffer quaternion_buffer_; 91 | Eigen::Quaterniond quaternion_ = Eigen::Quaterniond::Identity(); 92 | 93 | static void crc16_update(uint16_t* crc_src, const std::byte* bytes, uint32_t len) { 94 | uint32_t crc = *crc_src; 95 | for (std::size_t byte_index = 0; byte_index < len; ++byte_index) { 96 | auto byte = static_cast(bytes[byte_index]); 97 | crc ^= byte << 8; 98 | for (std::size_t crc_index = 0; crc_index < 8; ++crc_index) { 99 | uint32_t temp = crc << 1; 100 | if (crc & 0x8000) 101 | temp ^= 0x1021; 102 | crc = temp; 103 | } 104 | } 105 | *crc_src = crc; 106 | } 107 | }; 108 | 109 | } // namespace rmcs_core::hardware::device 110 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace rmcs_core::hardware::device { 9 | 10 | class LkMotor : public librmcs::device::LkMotor { 11 | public: 12 | LkMotor( 13 | rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, 14 | const std::string& name_prefix) 15 | : librmcs::device::LkMotor() { 16 | status_component.register_output(name_prefix + "/angle", angle_, 0.0); 17 | status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); 18 | status_component.register_output(name_prefix + "/torque", torque_, 0.0); 19 | status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); 20 | 21 | command_component.register_input( 22 | name_prefix + "/control_velocity", control_velocity_, false); 23 | } 24 | 25 | LkMotor( 26 | rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, 27 | const std::string& name_prefix, const Config& config) 28 | : LkMotor(status_component, command_component, name_prefix) { 29 | configure(config); 30 | } 31 | 32 | void configure(const Config& config) { 33 | librmcs::device::LkMotor::configure(config); 34 | 35 | *max_torque_ = max_torque(); 36 | } 37 | 38 | void update_status() { 39 | librmcs::device::LkMotor::update_status(); 40 | *angle_ = angle(); 41 | *velocity_ = velocity(); 42 | *torque_ = torque(); 43 | } 44 | 45 | double control_velocity() const { 46 | if (control_velocity_.ready()) [[likely]] 47 | return *control_velocity_; 48 | else 49 | return std::numeric_limits::quiet_NaN(); 50 | } 51 | 52 | uint64_t generate_command() { 53 | return librmcs::device::LkMotor::generate_velocity_command(control_velocity()); 54 | } 55 | 56 | private: 57 | rmcs_executor::Component::OutputInterface angle_; 58 | rmcs_executor::Component::OutputInterface velocity_; 59 | rmcs_executor::Component::OutputInterface torque_; 60 | rmcs_executor::Component::OutputInterface max_torque_; 61 | 62 | rmcs_executor::Component::InputInterface control_velocity_; 63 | }; 64 | 65 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/hardware/device/supercap.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace rmcs_core::hardware::device { 10 | using rmcs_executor::Component; 11 | 12 | class Supercap { 13 | public: 14 | explicit Supercap(Component& status_component, Component& command_component) { 15 | status_component.register_output("/chassis/power", chassis_power_, 0.0); 16 | status_component.register_output("/chassis/voltage", chassis_voltage_, 0.0); 17 | status_component.register_output("/chassis/supercap/voltage", supercap_voltage_, 0.0); 18 | status_component.register_output("/chassis/supercap/enabled", supercap_enabled_, false); 19 | 20 | command_component.register_input( 21 | "/chassis/supercap/control_enable", supercap_control_enabled_); 22 | command_component.register_input( 23 | "/chassis/supercap/charge_power_limit", supercap_charge_power_limit_); 24 | } 25 | 26 | void store_status(uint64_t can_data) { 27 | can_data_.store(std::bit_cast(can_data), std::memory_order::relaxed); 28 | } 29 | 30 | void update_status() { 31 | auto status = can_data_.load(std::memory_order::relaxed); 32 | 33 | *chassis_power_ = uint_to_double(status.chassis_power, 0.0, 500.0); 34 | *chassis_voltage_ = uint_to_double(status.chassis_voltage, 0.0, 50.0); 35 | *supercap_voltage_ = uint_to_double(status.supercap_voltage, 0.0, 50.0); 36 | *supercap_enabled_ = status.enabled; 37 | } 38 | 39 | uint16_t generate_command() const { 40 | SupercapCommand command; 41 | 42 | command.enabled = *supercap_control_enabled_; 43 | 44 | double power_limit = *supercap_charge_power_limit_; 45 | if (std::isnan(power_limit)) 46 | command.power_limit = 0; 47 | else 48 | command.power_limit = static_cast(std::clamp(power_limit, 0.0, 255.0)); 49 | 50 | return std::bit_cast(command); 51 | } 52 | 53 | private: 54 | static constexpr double 55 | uint_to_double(std::unsigned_integral auto value, double min, double max) { 56 | double span = max - min; 57 | double offset = min; 58 | return (double)value / (double)decltype(value)(-1) * span + offset; 59 | } 60 | 61 | struct __attribute__((packed, aligned(8))) SupercapStatus { 62 | uint16_t chassis_power; 63 | uint16_t supercap_voltage; 64 | uint16_t chassis_voltage; 65 | uint8_t enabled; 66 | uint8_t unused; 67 | }; 68 | std::atomic can_data_{}; 69 | static_assert(decltype(can_data_)::is_always_lock_free); 70 | 71 | struct __attribute__((packed, aligned(2))) SupercapCommand { 72 | uint8_t power_limit; 73 | bool enabled; 74 | }; 75 | 76 | Component::OutputInterface chassis_power_; 77 | Component::OutputInterface chassis_voltage_; 78 | Component::OutputInterface supercap_voltage_; 79 | Component::OutputInterface supercap_enabled_; 80 | 81 | Component::InputInterface supercap_control_enabled_; 82 | Component::InputInterface supercap_charge_power_limit_; 83 | }; 84 | 85 | } // namespace rmcs_core::hardware::device -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include "referee/app/ui/shape/red_black_tree.hpp" 8 | 9 | namespace rmcs_core::referee::app::ui { 10 | 11 | template 12 | class CfsScheduler { 13 | public: 14 | class __attribute__((packed, aligned(sizeof(void*)))) Entity 15 | : private RedBlackTree::Node { 16 | public: 17 | friend class CfsScheduler; 18 | friend class RedBlackTree; 19 | 20 | bool is_in_run_queue() requires(std::is_base_of_v) { 21 | return !RedBlackTree::Node::is_dangling(); 22 | } 23 | 24 | void enter_run_queue(uint16_t priority) requires(std::is_base_of_v) { 25 | if (this->priority_ != priority) { 26 | vruntime_ += this->priority_; 27 | vruntime_ -= priority; 28 | if (this->vruntime_ < min_vruntime_) 29 | this->vruntime_ = min_vruntime_; 30 | 31 | this->priority_ = priority; 32 | 33 | if (is_in_run_queue()) 34 | run_queue_.erase(*this); 35 | } else { 36 | if (is_in_run_queue()) 37 | return; 38 | } 39 | 40 | run_queue_.insert(*this); 41 | } 42 | 43 | void leave_run_queue() requires(std::is_base_of_v) { 44 | if (is_in_run_queue()) [[likely]] 45 | run_queue_.erase(*this); 46 | } 47 | 48 | private: 49 | bool operator<(const Entity& obj) const { return vruntime_ < obj.vruntime_; } 50 | uint64_t vruntime_ : 48 = 65536; 51 | uint16_t priority_ = 0; 52 | }; 53 | 54 | // class T : public Entity {}; 55 | 56 | class UpdateIterator { 57 | public: 58 | UpdateIterator() 59 | : current_(run_queue_.first()) 60 | , ignored_(nullptr) {} 61 | UpdateIterator(const UpdateIterator&) = delete; 62 | UpdateIterator& operator=(const UpdateIterator&) = delete; 63 | UpdateIterator(UpdateIterator&&) = default; 64 | UpdateIterator& operator=(UpdateIterator&&) = default; 65 | 66 | T* get() const { 67 | // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) 68 | return static_cast(current_); 69 | } 70 | 71 | T& operator*() const { return *get(); } 72 | T* operator->() const { return get(); } 73 | explicit operator bool() const { return get(); } 74 | 75 | auto update() { 76 | min_vruntime_ = current_->vruntime_; 77 | int shift = 65536 - current_->priority_; 78 | current_->vruntime_ += shift; 79 | 80 | run_queue_.erase(*current_); 81 | auto result = get()->update(); 82 | current_ = ignored_ ? ignored_->next() : run_queue_.first(); 83 | 84 | return result; 85 | } 86 | 87 | void ignore() { 88 | ignored_ = current_; 89 | current_ = ignored_->next(); 90 | } 91 | 92 | private: 93 | Entity *current_, *ignored_; 94 | }; 95 | 96 | static inline bool empty() { return run_queue_.empty(); } 97 | 98 | static inline UpdateIterator get_update_iterator() 99 | requires std::is_base_of_v && requires(T t) { t.update(); } { 100 | return UpdateIterator{}; 101 | } 102 | 103 | private: 104 | static inline RedBlackTree run_queue_; 105 | static inline uint64_t min_vruntime_ = 0; 106 | }; 107 | 108 | } // namespace rmcs_core::referee::app::ui -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "referee/app/ui/shape/red_black_tree.hpp" 6 | 7 | namespace rmcs_core::referee::app::ui { 8 | template 9 | class RemoteShape { 10 | public: 11 | class Descriptor : private RedBlackTree::Node { 12 | public: 13 | friend class RemoteShape; 14 | friend class RedBlackTree; 15 | 16 | Descriptor() = default; 17 | Descriptor(const Descriptor&) = delete; 18 | Descriptor& operator=(const Descriptor&) = delete; 19 | Descriptor(Descriptor&&) = delete; 20 | Descriptor& operator=(Descriptor&& obj) = delete; 21 | 22 | [[nodiscard]] bool has_id() const { return id_; } 23 | [[nodiscard]] bool try_assign_id() 24 | requires std::is_base_of_v && requires(T t) { t.id_revoked(); } { 25 | if (has_id()) [[unlikely]] 26 | return false; 27 | 28 | if (Descriptor* first = swapping_queue_.first()) { 29 | // Optimization: Try to find a descriptor to avoid creating a new one. 30 | swapping_queue_.erase(*first); 31 | swap_id(*first); 32 | return true; 33 | } 34 | 35 | if (next_id_ > id_assignment_max) [[unlikely]] 36 | return false; 37 | else { 38 | assign_id(); 39 | return true; 40 | } 41 | } 42 | [[nodiscard]] bool predict_try_assign_id(uint8_t& existence_confidence) const { 43 | if (has_id()) [[unlikely]] 44 | return false; 45 | 46 | if (Descriptor* first = swapping_queue_.first()) { 47 | existence_confidence = first->existence_confidence_; 48 | return true; 49 | } 50 | 51 | return next_id_ <= id_assignment_max; 52 | } 53 | 54 | [[nodiscard]] bool swapping_enabled() const { 55 | return !RedBlackTree::Node::is_dangling(); 56 | } 57 | void enable_swapping() { 58 | if (swapping_enabled()) 59 | return; 60 | swapping_queue_.insert(*this); 61 | } 62 | void disable_swapping() { 63 | if (!swapping_enabled()) 64 | return; 65 | swapping_queue_.erase(*this); 66 | } 67 | 68 | [[nodiscard]] uint8_t id() const { return id_; } 69 | [[nodiscard]] uint8_t existence_confidence() const { return existence_confidence_; } 70 | 71 | uint8_t increase_existence_confidence() { 72 | ++existence_confidence_; 73 | if (swapping_enabled()) { 74 | disable_swapping(), enable_swapping(); 75 | } 76 | return existence_confidence_; 77 | } 78 | 79 | private: 80 | /* Swap requirement: !this->id_ && victim.id_ */ 81 | void swap_id(Descriptor& victim) { 82 | id_ = victim.id_; 83 | assigned_list_[id_ - 1] = this; 84 | existence_confidence_ = victim.existence_confidence_; 85 | 86 | victim.revoke_id(); 87 | } 88 | 89 | /* Assign requirement: next_id_ <= id_assignment_max */ 90 | void assign_id() { 91 | id_ = next_id_++; 92 | 93 | assigned_list_[id_ - 1] = this; 94 | } 95 | 96 | void revoke_id() { 97 | id_ = 0; 98 | existence_confidence_ = 0; 99 | 100 | static_cast(this)->id_revoked(); 101 | } 102 | 103 | bool operator<(const Descriptor& obj) const { 104 | return existence_confidence_ < obj.existence_confidence_; 105 | } 106 | 107 | uint8_t id_ = 0; 108 | uint8_t existence_confidence_ = 0; 109 | }; 110 | 111 | static inline void force_revoke_all_id() { 112 | for (int i = 0; i < next_id_ - 1; ++i) { 113 | assigned_list_[i]->revoke_id(); 114 | } 115 | next_id_ = 1; 116 | } 117 | 118 | private: 119 | static constexpr uint8_t id_assignment_max = 201; 120 | 121 | static inline uint8_t next_id_ = 1; 122 | static inline Descriptor* assigned_list_[id_assignment_max]; 123 | 124 | static inline RedBlackTree swapping_queue_; 125 | }; 126 | } // namespace rmcs_core::referee::app::ui -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "referee/app/ui/shape/shape.hpp" 4 | 5 | namespace rmcs_core::referee::app::ui { 6 | 7 | class CrossHair { 8 | public: 9 | CrossHair(Shape::Color color, uint16_t x, uint16_t y, bool visible = true) 10 | : guidelines_( 11 | {color, 2, (uint16_t)(x - r2), y, (uint16_t)(x - r1), y, visible}, 12 | {color, 2, (uint16_t)(x + r1), y, (uint16_t)(x + r2), y, visible}, 13 | {color, 2, x, (uint16_t)(y + r2), x, (uint16_t)(y + r1), visible}, 14 | {color, 2, x, (uint16_t)(y - r1), x, (uint16_t)(y - r2), visible}) 15 | , center_(color, 2, x, y, 1, 1) {} 16 | 17 | void set_visible(bool value) { 18 | for (auto& line : guidelines_) 19 | line.set_visible(value); 20 | center_.set_visible(value); 21 | } 22 | 23 | private: 24 | static constexpr uint16_t r1 = 8, r2 = 24; 25 | 26 | Line guidelines_[4]; 27 | Circle center_; 28 | }; 29 | 30 | } // namespace rmcs_core::referee::app::ui -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "referee/command/field.hpp" 9 | #include "referee/frame.hpp" 10 | 11 | namespace rmcs_core::referee { 12 | using namespace command; 13 | 14 | class Command 15 | : public rmcs_executor::Component 16 | , public rclcpp::Node { 17 | public: 18 | Command() 19 | : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} 20 | , next_sent_(std::chrono::steady_clock::time_point::min()) 21 | , interaction_next_sent_(std::chrono::steady_clock::time_point::min()) 22 | , map_marker_next_sent_(std::chrono::steady_clock::time_point::min()) 23 | , text_display_next_sent_(std::chrono::steady_clock::time_point::min()) { 24 | 25 | register_input("/referee/serial", serial_, false); 26 | 27 | register_input("/referee/command/interaction", interaction_field_, false); 28 | register_input("/referee/command/map_marker", map_marker_field_, false); 29 | register_input("/referee/command/text_display", text_display_field_, false); 30 | } 31 | 32 | void before_updating() override { 33 | if (!interaction_field_.ready()) 34 | interaction_field_.bind_directly(empty_field_); 35 | if (!map_marker_field_.ready()) 36 | map_marker_field_.bind_directly(empty_field_); 37 | if (!text_display_field_.ready()) 38 | text_display_field_.bind_directly(empty_field_); 39 | } 40 | 41 | void update() override { 42 | if (!serial_.ready()) 43 | return; 44 | 45 | using namespace std::chrono_literals; 46 | auto now = std::chrono::steady_clock::now(); 47 | auto& serial = const_cast(*serial_); 48 | 49 | if (now < next_sent_) 50 | return; 51 | 52 | constexpr auto one_second = std::chrono::steady_clock::duration(1s); 53 | size_t data_length; 54 | if (now >= interaction_next_sent_ && !interaction_field_->empty()) { 55 | interaction_next_sent_ = now + (one_second / 25); // 25hz max to reduce packet loss 56 | frame_.body.command_id = 0x0301; 57 | data_length = interaction_field_->write(frame_.body.data); 58 | } else if (now >= map_marker_next_sent_ && !map_marker_field_->empty()) { 59 | map_marker_next_sent_ = now + (one_second / 1); // 1hz max 60 | frame_.body.command_id = 0x0307; 61 | data_length = map_marker_field_->write(frame_.body.data); 62 | } else if (now >= text_display_next_sent_ && !text_display_field_->empty()) { 63 | text_display_next_sent_ = now + (one_second / 3); // 3hz max 64 | frame_.body.command_id = 0x0308; 65 | data_length = text_display_field_->write(frame_.body.data); 66 | } else { 67 | return; 68 | } 69 | 70 | // TODO(qzh): Assert data length. 71 | 72 | frame_.header.sof = sof_value; 73 | frame_.header.data_length = data_length; 74 | frame_.header.sequence = 0; 75 | rmcs_utility::dji_crc::append_crc8(frame_.header); 76 | 77 | auto frame_size = 78 | sizeof(frame_.header) + sizeof(frame_.body.command_id) + data_length + sizeof(uint16_t); 79 | rmcs_utility::dji_crc::append_crc16(&frame_, frame_size); 80 | 81 | serial.write(reinterpret_cast(&frame_), frame_size); 82 | next_sent_ = now + (one_second / 3720 * frame_size); 83 | } 84 | 85 | private: 86 | InputInterface serial_; 87 | Frame frame_; 88 | 89 | Field empty_field_; 90 | std::chrono::steady_clock::time_point next_sent_; 91 | 92 | InputInterface interaction_field_; 93 | std::chrono::steady_clock::time_point interaction_next_sent_; 94 | 95 | InputInterface map_marker_field_; 96 | std::chrono::steady_clock::time_point map_marker_next_sent_; 97 | 98 | InputInterface text_display_field_; 99 | std::chrono::steady_clock::time_point text_display_next_sent_; 100 | }; 101 | 102 | } // namespace rmcs_core::referee 103 | 104 | #include 105 | 106 | PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Command, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/field.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace rmcs_core::referee::command { 12 | 13 | class Field { 14 | public: 15 | constexpr Field() { write_ = &do_nothing; } 16 | 17 | template 18 | requires requires(const F& f, std::byte* buffer) { 19 | { f(buffer) } -> std::convertible_to; 20 | } constexpr explicit Field(F functor) { 21 | static_assert(sizeof(functor) <= sizeof(intptr_t)); 22 | static_assert(std::is_trivially_destructible_v); 23 | 24 | ::new (&storage_) F(std::move(functor)); 25 | write_ = [](const intptr_t& storage, std::byte* buffer) { 26 | return (*std::launder(reinterpret_cast(&storage)))(buffer); 27 | }; 28 | } 29 | 30 | [[nodiscard]] constexpr explicit operator bool() const { return write_ != &do_nothing; } 31 | [[nodiscard]] constexpr bool empty() const { return write_ == &do_nothing; } 32 | 33 | size_t write(std::byte* buffer) const { return write_(storage_, buffer); }; 34 | 35 | private: 36 | static constexpr size_t do_nothing(const intptr_t&, std::byte*) { return 0; }; 37 | 38 | intptr_t storage_; 39 | size_t (*write_)(const intptr_t&, std::byte*); 40 | }; 41 | 42 | inline size_t write_field(std::byte*) { return 0; } 43 | 44 | template 45 | inline size_t write_field(std::byte*, const Field&, const Ts&...); 46 | template 47 | inline size_t write_field(std::byte*, const T&, const Ts&...); 48 | 49 | template 50 | inline size_t write_field(std::byte* buffer, const Field& package, const Ts&... other_data) { 51 | auto written = package.write(buffer); 52 | return written + write_field(buffer + written, other_data...); 53 | } 54 | template 55 | inline size_t write_field(std::byte* buffer, const T& data, const Ts&... other_data) { 56 | memcpy(buffer, &data, sizeof(data)); 57 | return sizeof(data) + write_field(buffer + sizeof(data), other_data...); 58 | } 59 | 60 | #define MAKE_FIELD(...) \ 61 | ::rmcs_core::referee::command::Field { \ 62 | [this](std::byte* buffer) { return write_field(buffer, __VA_ARGS__); } \ 63 | } 64 | 65 | } // namespace rmcs_core::referee::command -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/interaction.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "referee/command/field.hpp" 5 | 6 | namespace rmcs_core::referee::command { 7 | 8 | class Interaction 9 | : public rmcs_executor::Component 10 | , public rclcpp::Node { 11 | public: 12 | Interaction() 13 | : Node{ 14 | get_component_name(), 15 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { 16 | 17 | register_input( 18 | "/referee/command/interaction/sentry_decision", sentry_decision_field_, false); 19 | register_input("/referee/command/interaction/communicate", communicate_field_, false); 20 | register_input("/referee/command/interaction/ui", ui_field_, false); 21 | 22 | register_output("/referee/command/interaction", interaction_field_); 23 | } 24 | 25 | void before_updating() override { 26 | if (!sentry_decision_field_.ready()) 27 | sentry_decision_field_.bind_directly(empty_field_); 28 | if (!communicate_field_.ready()) 29 | communicate_field_.bind_directly(empty_field_); 30 | if (!ui_field_.ready()) 31 | ui_field_.bind_directly(empty_field_); 32 | } 33 | 34 | void update() override { 35 | if (*sentry_decision_field_) 36 | *interaction_field_ = *sentry_decision_field_; 37 | else if (*communicate_field_) 38 | *interaction_field_ = *communicate_field_; 39 | else if (*ui_field_) 40 | *interaction_field_ = *ui_field_; 41 | else 42 | *interaction_field_ = Field{}; 43 | } 44 | 45 | private: 46 | Field empty_field_; 47 | 48 | InputInterface sentry_decision_field_; 49 | InputInterface communicate_field_; 50 | InputInterface ui_field_; 51 | 52 | struct __attribute__((packed)) InteractionHeader { 53 | uint16_t command_id; 54 | uint16_t sender_id; 55 | uint16_t receiver_id; 56 | }; 57 | 58 | OutputInterface interaction_field_; 59 | }; 60 | 61 | } // namespace rmcs_core::referee::command 62 | 63 | #include 64 | 65 | PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::Interaction, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/interaction/communicate.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/communicate.cpp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace rmcs_core::referee::command::interaction { 8 | 9 | struct __attribute__((packed)) Header { 10 | uint16_t command_id; 11 | uint16_t sender_id; 12 | uint16_t receiver_id; 13 | }; 14 | 15 | } // namespace rmcs_core::referee::command::interaction -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "referee/app/ui/shape/cfs_scheduler.hpp" 11 | #include "referee/app/ui/shape/shape.hpp" 12 | #include "referee/command/interaction/header.hpp" 13 | 14 | namespace rmcs_core::referee::command::interaction { 15 | using namespace app::ui; 16 | 17 | class Ui 18 | : public rmcs_executor::Component 19 | , public rclcpp::Node { 20 | public: 21 | Ui() 22 | : Node{ 23 | get_component_name(), 24 | rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { 25 | 26 | register_input("/referee/id", robot_id_); 27 | register_input("/referee/game/stage", game_stage_); 28 | register_input("/remote/keyboard", keyboard_); 29 | 30 | register_output("/referee/command/interaction/ui", ui_field_); 31 | } 32 | 33 | void update() override { 34 | if (*robot_id_ == rmcs_msgs::RobotId::UNKNOWN) { 35 | *ui_field_ = Field{}; 36 | return; 37 | } 38 | 39 | if ((last_game_stage_ == rmcs_msgs::GameStage::UNKNOWN 40 | && *game_stage_ != rmcs_msgs::GameStage::UNKNOWN) 41 | || (last_game_stage_ != rmcs_msgs::GameStage::PREPARATION 42 | && *game_stage_ == rmcs_msgs::GameStage::PREPARATION) 43 | || (!last_keyboard_.r && keyboard_->r)) { 44 | RemoteShape::force_revoke_all_id(); 45 | resetting_ = 4; 46 | } 47 | last_game_stage_ = *game_stage_; 48 | last_keyboard_ = *keyboard_; 49 | 50 | if (resetting_) { 51 | *ui_field_ = Field{[this](std::byte* buffer) { 52 | --resetting_; 53 | return write_resetting_field(buffer); 54 | }}; 55 | return; 56 | } 57 | 58 | if (CfsScheduler::empty()) { 59 | *ui_field_ = Field{}; 60 | return; 61 | } 62 | 63 | *ui_field_ = Field{[this](std::byte* buffer) { return write_updating_field(buffer); }}; 64 | } 65 | 66 | private: 67 | size_t write_resetting_field(std::byte* buffer) const { 68 | size_t written = 0; 69 | 70 | auto& header = *new (buffer + written) Header{}; 71 | header.command_id = 0x0100; // Clear shapes 72 | auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; 73 | header.sender_id = full_robot_id; 74 | header.receiver_id = full_robot_id.client(); 75 | written += sizeof(Header); 76 | 77 | struct Command { 78 | uint8_t type; 79 | uint8_t layer; 80 | }; 81 | auto& command = *new (buffer + written) Command{}; 82 | command.type = 2; // Clear all layers 83 | command.layer = 0; 84 | written += sizeof(Command); 85 | 86 | return written; 87 | } 88 | 89 | size_t write_updating_field(std::byte* buffer) const { 90 | size_t written = 0; 91 | 92 | auto& header = *new (buffer + written) Header{}; 93 | auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; 94 | header.sender_id = full_robot_id; 95 | header.receiver_id = full_robot_id.client(); 96 | written += sizeof(Header); 97 | 98 | int slot = 0; 99 | intptr_t updated[7]; 100 | for (auto it = CfsScheduler::get_update_iterator(); it && slot < 7;) { 101 | // Ignore text shape unless it is the first. 102 | if (it->is_text_shape()) { 103 | if (slot == 0) { 104 | header.command_id = 0x0110; // Draw text shape 105 | return written + it.update().write(buffer + written); 106 | } else { 107 | it.ignore(); 108 | continue; 109 | } 110 | } 111 | 112 | auto operation = it->predict_update(); 113 | if (operation == Shape::Operation::NO_OPERATION) { 114 | it.ignore(); 115 | continue; 116 | } 117 | 118 | // Shapes are always aligned, so the last bits can be used to store information. 119 | auto identification = 120 | reinterpret_cast(it.get()) | (operation == Shape::Operation::ADD); 121 | // Ignore identical shapes that operate identically. 122 | if (std::find(updated, updated + slot, identification) != updated + slot) { 123 | it.ignore(); 124 | continue; 125 | } 126 | 127 | written += it.update().write(buffer + written); 128 | 129 | updated[slot++] = identification; 130 | } 131 | 132 | constexpr std::pair optional_packet[4] = { 133 | {1, 0x0101}, // Draw 1 shape 134 | {2, 0x0102}, // Draw 2 shapes 135 | {5, 0x0103}, // Draw 5 shapes 136 | {7, 0x0104}, // Draw 7 shapes 137 | }; 138 | for (const auto& [shape_count, command_id] : optional_packet) { 139 | if (slot <= shape_count) { 140 | for (; slot < shape_count; ++slot) { 141 | written += Shape::no_operation_description().write(buffer + written); 142 | } 143 | header.command_id = command_id; 144 | break; 145 | } 146 | } 147 | 148 | return written; 149 | } 150 | 151 | InputInterface robot_id_; 152 | 153 | InputInterface game_stage_; 154 | rmcs_msgs::GameStage last_game_stage_ = rmcs_msgs::GameStage::UNKNOWN; 155 | 156 | InputInterface keyboard_; 157 | rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); 158 | 159 | int resetting_ = 0; 160 | 161 | OutputInterface ui_field_; 162 | }; 163 | 164 | } // namespace rmcs_core::referee::command::interaction 165 | 166 | #include 167 | 168 | PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/map_marker.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_core/src/referee/command/map_marker.cpp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/command/text_display.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_core/src/referee/command/text_display.cpp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/frame.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_core::referee { 7 | 8 | constexpr uint8_t sof_value = 0xa5; 9 | constexpr size_t frame_data_max_length = 1024; 10 | 11 | struct __attribute__((packed)) FrameHeader { 12 | uint8_t sof; 13 | uint16_t data_length; 14 | uint8_t sequence; 15 | uint8_t crc8; 16 | }; 17 | 18 | struct __attribute__((packed)) FrameBody { 19 | uint16_t command_id; 20 | std::byte data[frame_data_max_length]; 21 | }; 22 | 23 | struct __attribute__((packed)) Frame { 24 | FrameHeader header; 25 | FrameBody body; 26 | }; 27 | 28 | } // namespace rmcs_core::referee -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_core/src/referee/status/field.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_core::referee::status { 6 | 7 | struct __attribute__((packed)) GameStatus { 8 | uint8_t game_type : 4; 9 | uint8_t game_stage : 4; 10 | uint16_t stage_remain_time; 11 | uint64_t sync_timestamp; 12 | }; 13 | 14 | struct __attribute__((packed)) GameRobotHp { 15 | uint16_t red_1; 16 | uint16_t red_2; 17 | uint16_t red_3; 18 | uint16_t red_4; 19 | uint16_t red_5; 20 | uint16_t red_7; 21 | uint16_t red_outpost; 22 | uint16_t red_base; 23 | uint16_t blue_1; 24 | uint16_t blue_2; 25 | uint16_t blue_3; 26 | uint16_t blue_4; 27 | uint16_t blue_5; 28 | uint16_t blue_7; 29 | uint16_t blue_outpost; 30 | uint16_t blue_base; 31 | }; 32 | 33 | struct __attribute__((packed)) RobotStatus { 34 | uint8_t robot_id; 35 | uint8_t robot_level; 36 | uint16_t current_hp; 37 | uint16_t maximum_hp; 38 | uint16_t shooter_barrel_cooling_value; 39 | uint16_t shooter_barrel_heat_limit; 40 | uint16_t chassis_power_limit; 41 | uint8_t power_management_gimbal_output : 1; 42 | uint8_t power_management_chassis_output : 1; 43 | uint8_t power_management_shooter_output : 1; 44 | }; 45 | 46 | struct __attribute__((packed)) PowerHeatData { 47 | uint16_t chassis_voltage; 48 | uint16_t chassis_current; 49 | float chassis_power; 50 | uint16_t buffer_energy; 51 | uint16_t shooter_17mm_1_barrel_heat; 52 | uint16_t shooter_17mm_2_barrel_heat; 53 | uint16_t shooter_42mm_barrel_heat; 54 | }; 55 | 56 | struct __attribute__((packed)) RobotPosition { 57 | float x; 58 | float y; 59 | float angle; 60 | }; 61 | 62 | struct __attribute__((packed)) HurtData { 63 | uint8_t armor_id : 4; 64 | uint8_t reason : 4; 65 | }; 66 | 67 | struct __attribute__((packed)) ShootData { 68 | uint8_t bullet_type; 69 | uint8_t shooter_number; 70 | uint8_t launching_frequency; 71 | float initial_speed; 72 | }; 73 | 74 | struct __attribute__((packed)) BulletAllowance { 75 | uint16_t bullet_allowance_17mm; 76 | uint16_t bullet_allowance_42mm; 77 | uint16_t remaining_gold_coin; 78 | }; 79 | 80 | struct __attribute__((packed)) GameRobotPosition { 81 | float hero_x; 82 | float hero_y; 83 | float engineer_x; 84 | float engineer_y; 85 | float infantry_3_x; 86 | float infantry_3_y; 87 | float infantry_4_x; 88 | float infantry_4_y; 89 | float infantry_5_x; 90 | float infantry_5_y; 91 | }; 92 | 93 | } // namespace rmcs_core::referee::status -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(rmcs_description) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-O2 -Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package (ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies () 14 | 15 | file (GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS 16 | ${PROJECT_SOURCE_DIR}/src/*.cpp 17 | ${PROJECT_SOURCE_DIR}/src/*.c 18 | ) 19 | 20 | ament_auto_add_library ( 21 | ${PROJECT_NAME} 22 | SHARED 23 | ${PROJECT_SOURCE} 24 | ) 25 | 26 | include_directories(${PROJECT_SOURCE_DIR}/include) 27 | include_directories(${PROJECT_SOURCE_DIR}/src) 28 | 29 | install( 30 | DIRECTORY config launch meshes rviz urdf 31 | DESTINATION share/${PROJECT_NAME} 32 | ) 33 | 34 | ament_auto_package() -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/config/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/config/.gitkeep -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/include/rmcs_description/tf_description.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_description { 7 | 8 | struct BaseLink : fast_tf::Link { 9 | static constexpr char name[] = "base_link"; 10 | }; 11 | 12 | struct YawLink : fast_tf::Link { 13 | static constexpr char name[] = "yaw_link"; 14 | }; 15 | struct PitchLink : fast_tf::Link { 16 | static constexpr char name[] = "pitch_link"; 17 | }; 18 | 19 | struct MuzzleLink : fast_tf::Link { 20 | static constexpr char name[] = "muzzle_link"; 21 | }; 22 | 23 | struct CameraLink : fast_tf::Link { 24 | static constexpr char name[] = "camera_link"; 25 | }; 26 | 27 | struct TransmitterLink : fast_tf::Link { 28 | static constexpr char name[] = "transmitter_link"; 29 | }; 30 | 31 | struct OdomImu : fast_tf::Link { 32 | static constexpr char name[] = "odom_imu"; 33 | }; 34 | 35 | struct GimbalCenterLink : fast_tf::Link { 36 | static constexpr char name[] = "gimbal_center_link"; 37 | }; 38 | struct LeftFrontWheelLink : fast_tf::Link { 39 | static constexpr char name[] = "left_front_wheel_link"; 40 | }; 41 | struct LeftBackWheelLink : fast_tf::Link { 42 | static constexpr char name[] = "left_back_wheel_link"; 43 | }; 44 | struct RightBackWheelLink : fast_tf::Link { 45 | static constexpr char name[] = "right_back_wheel_link"; 46 | }; 47 | struct RightFrontWheelLink : fast_tf::Link { 48 | static constexpr char name[] = "right_front_wheel_link"; 49 | }; 50 | 51 | struct OmniLinkLeftFront : fast_tf::Link { 52 | static constexpr char name[] = "omni_link_left_front"; 53 | }; 54 | 55 | struct OmniLinkRightFront : fast_tf::Link { 56 | static constexpr char name[] = "omni_link_right_front"; 57 | }; 58 | 59 | struct OmniLinkLeft : fast_tf::Link { 60 | static constexpr char name[] = "omni_link_left"; 61 | }; 62 | 63 | struct OmniLinkRight : fast_tf::Link { 64 | static constexpr char name[] = "omni_link_right"; 65 | }; 66 | 67 | } // namespace rmcs_description 68 | 69 | template <> 70 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 71 | using Parent = rmcs_description::BaseLink; 72 | Eigen::Translation3d transform = Eigen::Translation3d::Identity(); 73 | }; 74 | 75 | template <> 76 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 77 | using Parent = rmcs_description::GimbalCenterLink; 78 | 79 | void set_state(double angle) { angle_ = angle; } 80 | auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitZ()}; } 81 | 82 | private: 83 | double angle_; 84 | }; 85 | 86 | template <> 87 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 88 | using Parent = rmcs_description::YawLink; 89 | 90 | void set_state(double angle) { angle_ = angle; } 91 | auto get_transform() const { return Eigen::AngleAxisd{angle_, Eigen::Vector3d::UnitY()}; } 92 | 93 | private: 94 | double angle_; 95 | }; 96 | 97 | template <> 98 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 99 | using Parent = rmcs_description::PitchLink; 100 | Eigen::Translation3d transform = Eigen::Translation3d::Identity(); 101 | }; 102 | 103 | template <> 104 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 105 | using Parent = rmcs_description::PitchLink; 106 | Eigen::Translation3d transform = Eigen::Translation3d::Identity(); 107 | }; 108 | 109 | template <> 110 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 111 | using Parent = rmcs_description::PitchLink; 112 | Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); 113 | }; 114 | 115 | template <> 116 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 117 | using Parent = rmcs_description::PitchLink; 118 | Eigen::Quaterniond transform = Eigen::Quaterniond::Identity(); 119 | }; 120 | 121 | template <> 122 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 123 | using Parent = rmcs_description::BaseLink; 124 | Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); 125 | void set_state(double angle) { 126 | auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} 127 | * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; 128 | transform.linear() = rotation.matrix(); 129 | } 130 | }; 131 | 132 | template <> 133 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 134 | using Parent = rmcs_description::BaseLink; 135 | Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); 136 | void set_state(double angle) { 137 | auto rotation = Eigen::AngleAxisd{std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} 138 | * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; 139 | transform.linear() = rotation.matrix(); 140 | } 141 | }; 142 | 143 | template <> 144 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 145 | using Parent = rmcs_description::BaseLink; 146 | Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); 147 | void set_state(double angle) { 148 | auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4 * 3, Eigen::Vector3d::UnitZ()} 149 | * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; 150 | transform.linear() = rotation.matrix(); 151 | } 152 | }; 153 | 154 | template <> 155 | struct fast_tf::Joint : fast_tf::ModificationTrackable { 156 | using Parent = rmcs_description::BaseLink; 157 | Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); 158 | void set_state(double angle) { 159 | auto rotation = Eigen::AngleAxisd{-std::numbers::pi / 4, Eigen::Vector3d::UnitZ()} 160 | * Eigen::AngleAxisd{angle, Eigen::Vector3d::UnitX()}; 161 | transform.linear() = rotation.matrix(); 162 | } 163 | }; 164 | 165 | namespace rmcs_description { 166 | 167 | using Tf = fast_tf::JointCollection< 168 | GimbalCenterLink, YawLink, PitchLink, MuzzleLink, TransmitterLink, CameraLink, OdomImu, 169 | LeftFrontWheelLink, LeftBackWheelLink, RightBackWheelLink, RightFrontWheelLink>; 170 | 171 | using InfantryTf = fast_tf::JointCollection< 172 | GimbalCenterLink, YawLink, PitchLink, MuzzleLink, TransmitterLink, CameraLink, OdomImu, 173 | LeftFrontWheelLink, LeftBackWheelLink, RightBackWheelLink, RightFrontWheelLink>; 174 | 175 | using AutoAimTf = fast_tf::JointCollection; 176 | 177 | } // namespace rmcs_description 178 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/launch/display.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription 3 | from launch.substitutions import LaunchConfiguration, PathJoinSubstitution 4 | from launch_ros.substitutions import FindPackageShare 5 | 6 | 7 | def generate_launch_description(): 8 | ld = LaunchDescription() 9 | 10 | rmcs_description_path = FindPackageShare('rmcs_description') 11 | default_model_path = PathJoinSubstitution(['urdf', 'medium_feed_omni_wheel.xacro']) 12 | default_rviz_config_path = PathJoinSubstitution([rmcs_description_path, 'rviz', 'display.rviz']) 13 | 14 | # These parameters are maintained for backwards compatibility 15 | gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'], 16 | description='Flag to enable joint_state_publisher_gui') 17 | ld.add_action(gui_arg) 18 | rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path, 19 | description='Absolute path to rviz config file') 20 | ld.add_action(rviz_arg) 21 | 22 | # This parameter has changed its meaning slightly from previous versions 23 | ld.add_action(DeclareLaunchArgument(name='model', default_value=default_model_path, 24 | description='Path to robot urdf file relative to rmcs_description package')) 25 | 26 | ld.add_action(IncludeLaunchDescription( 27 | PathJoinSubstitution([FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']), 28 | launch_arguments={ 29 | 'urdf_package': 'rmcs_description', 30 | 'urdf_package_path': LaunchConfiguration('model'), 31 | 'rviz_config': LaunchConfiguration('rvizconfig'), 32 | 'jsp_gui': LaunchConfiguration('gui')}.items() 33 | )) 34 | 35 | return ld 36 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/meshes/collision/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/meshes/collision/.gitkeep -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/meshes/visual/base_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/meshes/visual/base_link.stl -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/meshes/visual/pitch_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/meshes/visual/pitch_link.stl -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/meshes/visual/wheel_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/meshes/visual/wheel_link.stl -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/meshes/visual/yaw_link.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/meshes/visual/yaw_link.stl -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | rmcs_description 6 | 0.0.0 7 | description 8 | Qzh 9 | GPL-3.0-only 10 | 11 | ament_cmake 12 | 13 | fast_tf 14 | 15 | joint_state_publisher_gui 16 | robot_state_publisher 17 | rviz2 18 | xacro 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/rviz/display.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Name: Displays 4 | - Class: rviz_common/Views 5 | Name: Views 6 | Visualization Manager: 7 | Displays: 8 | - Class: rviz_default_plugins/Grid 9 | Name: Grid 10 | Value: true 11 | - Alpha: 1 12 | Class: rviz_default_plugins/RobotModel 13 | Description Topic: 14 | Value: /robot_description 15 | Name: RobotModel 16 | Value: true 17 | - Class: rviz_default_plugins/TF 18 | Marker Scale: 0.5 19 | Name: TF 20 | Value: true 21 | Global Options: 22 | Fixed Frame: base_link 23 | Tools: 24 | - Class: rviz_default_plugins/MoveCamera 25 | Value: true 26 | Views: 27 | Current: 28 | Class: rviz_default_plugins/Orbit 29 | Distance: 1.7 30 | Name: Current View 31 | Pitch: 0.6 32 | Value: Orbit (rviz) 33 | Yaw: 2.24 34 | Window Geometry: 35 | Height: 800 36 | Width: 1200 37 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/src/tf_description.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alliance-Algorithm/RMCS/a2666ea08ae4ddfb795bfaec2a299cc70466012a/rmcs_ws/src/rmcs_description/src/tf_description.cpp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_description/urdf/medium_feed_omni_wheel.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 46 | 47 | 49 | 50 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(rmcs_executor) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-O2 -Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package (ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies () 14 | 15 | ament_auto_add_library ( 16 | ${PROJECT_NAME}_lib 17 | SHARED 18 | src/component.cpp 19 | ) 20 | 21 | ament_auto_add_executable ( 22 | ${PROJECT_NAME}_exe 23 | src/main.cpp 24 | ) 25 | target_link_libraries(${PROJECT_NAME}_exe ${PROJECT_NAME}_lib) 26 | 27 | include_directories(${PROJECT_SOURCE_DIR}/include) 28 | include_directories(${PROJECT_SOURCE_DIR}/src) 29 | 30 | set_property(TARGET ${PROJECT_NAME}_lib PROPERTY OUTPUT_NAME ${PROJECT_NAME}) 31 | set_property(TARGET ${PROJECT_NAME}_exe PROPERTY OUTPUT_NAME ${PROJECT_NAME}) 32 | 33 | ament_auto_package() 34 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/README.md: -------------------------------------------------------------------------------- 1 | Note: You must ensure that the activated interface is stored as a member 2 | variable of the component and deconstructed alongside the class. Failure to do 3 | so may lead to segmentation faults or memory out-of-bounds errors during 4 | execution. -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace rmcs_executor { 14 | 15 | class Component { 16 | public: 17 | friend class Executor; 18 | 19 | Component(const Component&) = delete; 20 | Component& operator=(const Component&) = delete; 21 | Component(Component&&) = delete; 22 | Component& operator=(Component&&) = delete; 23 | 24 | virtual ~Component(){}; 25 | 26 | virtual void before_pairing(const std::map& output_map) { 27 | (void)output_map; 28 | } 29 | virtual void before_updating() {} 30 | 31 | virtual void update() = 0; 32 | 33 | template 34 | requires(!std::is_reference_v && !std::is_unbounded_array_v) class InputInterface { 35 | public: 36 | friend class Component; 37 | 38 | InputInterface() = default; 39 | 40 | InputInterface(const InputInterface&) = delete; 41 | InputInterface& operator=(const InputInterface&) = delete; 42 | InputInterface(InputInterface&&) = delete; 43 | InputInterface& operator=(InputInterface&&) = delete; 44 | 45 | ~InputInterface() { 46 | if (delete_data_when_deconstruct) { 47 | if constexpr (std::is_array_v) { 48 | delete[] data_pointer_; 49 | } else { 50 | delete data_pointer_; 51 | } 52 | } 53 | } 54 | 55 | [[nodiscard]] bool active() const { return activated; } 56 | [[nodiscard]] bool ready() const { return data_pointer_ != nullptr; } 57 | 58 | template 59 | void make_and_bind_directly(Args&&... args) { 60 | if (ready()) 61 | throw std::runtime_error("The interface has already been bound to somewhere"); 62 | 63 | data_pointer_ = new T(std::forward(args)...); 64 | activated = true; 65 | 66 | delete_data_when_deconstruct = true; 67 | } 68 | 69 | void bind_directly(T& destination) { 70 | if (ready()) 71 | throw std::runtime_error("The interface has already been bound to somewhere"); 72 | 73 | data_pointer_ = &destination; 74 | activated = true; 75 | } 76 | 77 | const T* operator->() const { return data_pointer_; } 78 | const T& operator*() const { return *data_pointer_; } 79 | 80 | private: 81 | void** activate() { 82 | activated = true; 83 | return reinterpret_cast(&data_pointer_); 84 | } 85 | 86 | T* data_pointer_ = nullptr; 87 | bool activated = false; 88 | 89 | bool delete_data_when_deconstruct = false; 90 | }; 91 | 92 | template 93 | requires(!std::is_reference_v && !std::is_unbounded_array_v) class OutputInterface { 94 | public: 95 | friend class Component; 96 | 97 | OutputInterface() = default; 98 | 99 | OutputInterface(const OutputInterface&) = delete; 100 | OutputInterface& operator=(const OutputInterface&) = delete; 101 | OutputInterface(OutputInterface&&) = delete; 102 | OutputInterface& operator=(OutputInterface&&) = delete; 103 | 104 | ~OutputInterface() { 105 | if (active()) 106 | std::destroy_at(std::launder(reinterpret_cast(&data_))); 107 | }; 108 | 109 | [[nodiscard]] bool active() const { return activated; } 110 | 111 | T* operator->() { return reinterpret_cast(&data_); } 112 | const T* operator->() const { return reinterpret_cast(&data_); } 113 | T& operator*() { return *reinterpret_cast(&data_); } 114 | const T& operator*() const { return *reinterpret_cast(&data_); } 115 | 116 | private: 117 | template 118 | void* activate(Args&&... args) { 119 | ::new (&data_) T(std::forward(args)...); 120 | activated = true; 121 | return reinterpret_cast(&data_); 122 | } 123 | 124 | std::aligned_storage_t data_; 125 | bool activated = false; 126 | }; 127 | 128 | const std::string& get_component_name() { return component_name_; } 129 | 130 | template 131 | void register_input( 132 | const std::string& name, InputInterface& interface, bool required = true) { 133 | if (interface.active()) 134 | throw std::runtime_error("The interface has been activated"); 135 | input_list_.emplace_back(typeid(T), name, required, interface.activate()); 136 | } 137 | 138 | template 139 | void register_output(const std::string& name, OutputInterface& interface, Args&&... args) { 140 | if (interface.active()) 141 | throw std::runtime_error("The interface has been activated"); 142 | output_list_.emplace_back( 143 | typeid(T), name, interface.activate(std::forward(args)...), this); 144 | } 145 | 146 | template 147 | std::shared_ptr create_partner_component(const std::string& name, Args&&... args) { 148 | initializing_component_name = name.c_str(); 149 | 150 | auto component = std::make_shared(std::forward(args)...); 151 | partner_component_list_.emplace_back(component); 152 | 153 | return component; 154 | } 155 | 156 | static const char* initializing_component_name; 157 | 158 | protected: 159 | Component() 160 | : component_name_(initializing_component_name) {} 161 | 162 | private: 163 | std::string component_name_; 164 | 165 | struct InputDeclaration { 166 | const std::type_info& type; 167 | std::string name; 168 | bool required; 169 | void** pointer_to_data_pointer; 170 | }; 171 | 172 | struct OutputDeclaration { 173 | const std::type_info& type; 174 | std::string name; 175 | void* data_pointer; 176 | 177 | Component* component; 178 | }; 179 | 180 | std::vector input_list_; 181 | std::vector output_list_; 182 | 183 | std::vector> partner_component_list_; 184 | 185 | size_t dependency_count_ = 0; 186 | std::unordered_set wanted_by_ = {}; 187 | }; 188 | 189 | } // namespace rmcs_executor -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcs_executor 5 | 1.0.0 6 | RMCS Executor 7 | Qzh 8 | GPL-3.0-only 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | pluginlib 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/src/component.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcs_executor/component.hpp" 2 | 3 | namespace rmcs_executor { 4 | 5 | const char* Component::initializing_component_name; 6 | 7 | } // namespace rmcs_executor -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "executor.hpp" 7 | #include "rmcs_executor/component.hpp" 8 | 9 | int main(int argc, char** argv) { 10 | rclcpp::init(argc, argv); 11 | 12 | pluginlib::ClassLoader component_loader( 13 | "rmcs_executor", "rmcs_executor::Component"); 14 | 15 | rclcpp::executors::SingleThreadedExecutor rcl_executor; 16 | auto executor = std::make_shared("rmcs_executor", rcl_executor); 17 | rcl_executor.add_node(executor); 18 | 19 | std::vector component_descriptions; 20 | if (!executor->get_parameter("components", component_descriptions)) 21 | throw std::runtime_error("para"); 22 | 23 | std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); 24 | for (const auto& component_description : component_descriptions) { 25 | std::smatch matches; 26 | std::string plugin_name, component_name; 27 | 28 | if (std::regex_search(component_description, matches, regex)) { 29 | if (matches.size() != 3) 30 | throw std::runtime_error("In regex matching: unexpected number of matches"); 31 | 32 | plugin_name = matches[1].str(); 33 | component_name = matches[2].str(); 34 | } else { 35 | plugin_name = component_name = component_description; 36 | } 37 | 38 | rmcs_executor::Component::initializing_component_name = component_name.c_str(); 39 | auto component = component_loader.createSharedInstance(plugin_name); 40 | executor->add_component(component); 41 | } 42 | 43 | executor->start(); 44 | rcl_executor.spin(); 45 | 46 | rclcpp::shutdown(); 47 | } -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_executor/src/predefined_msg_provider.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "rmcs_executor/component.hpp" 4 | #include 5 | 6 | class PredefinedMsgProvider : public rmcs_executor::Component { 7 | public: 8 | PredefinedMsgProvider() { 9 | register_output("/predefined/update_rate", update_rate_); 10 | register_output("/predefined/update_count", update_count_, static_cast(-1)); 11 | register_output("/predefined/timestamp", timestamp_); 12 | } 13 | 14 | void set_update_rate(double frame_rate) { *update_rate_ = frame_rate; } 15 | void set_timestamp(std::chrono::steady_clock::time_point timestamp) { *timestamp_ = timestamp; } 16 | 17 | void update() override { *update_count_ += 1; } 18 | 19 | private: 20 | OutputInterface update_rate_; 21 | OutputInterface update_count_; 22 | OutputInterface timestamp_; 23 | }; -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(rmcs_msgs) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-O2 -Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package(ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies() 14 | 15 | file(GLOB RMCS_MSGS_MSGS "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg") 16 | 17 | foreach(MSG ${RMCS_MSGS_MSGS}) 18 | get_filename_component(MSG_NAME ${MSG} NAME_WE) 19 | set(MSG_PATH "${MSG_PATH};msg/${MSG_NAME}.msg") 20 | endforeach() 21 | 22 | rosidl_generate_interfaces(${PROJECT_NAME} 23 | ${MSG_PATH} 24 | DEPENDENCIES builtin_interfaces geometry_msgs 25 | ) 26 | 27 | include_directories(${PROJECT_SOURCE_DIR}/include) 28 | 29 | ament_auto_package() -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_msgs { 6 | 7 | enum class ChassisMode : uint8_t { 8 | AUTO = 0, 9 | SPIN = 1, 10 | STEP_DOWN = 2, 11 | LAUNCH_RAMP = 3, 12 | }; 13 | 14 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/full_robot_id.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "robot_color.hpp" 6 | #include "robot_id.hpp" 7 | 8 | namespace rmcs_msgs { 9 | 10 | class FullRobotId { 11 | public: 12 | enum Value : uint16_t { 13 | UNKNOWN = 0, 14 | 15 | RED_HERO = 1, 16 | RED_ENGINEER = 2, 17 | RED_INFANTRY_III = 3, 18 | RED_INFANTRY_IV = 4, 19 | RED_INFANTRY_V = 5, 20 | RED_AERIAL = 6, 21 | RED_SENTRY = 7, 22 | RED_DART = 8, 23 | RED_RADAR = 9, 24 | RED_OUTPOST = 10, 25 | RED_BASE = 11, 26 | 27 | BLUE_HERO = 101, 28 | BLUE_ENGINEER = 102, 29 | BLUE_INFANTRY_III = 103, 30 | BLUE_INFANTRY_IV = 104, 31 | BLUE_INFANTRY_V = 105, 32 | BLUE_AERIAL = 106, 33 | BLUE_SENTRY = 107, 34 | BLUE_DART = 108, 35 | BLUE_RADAR = 109, 36 | BLUE_OUTPOST = 110, 37 | BLUE_BASE = 111, 38 | 39 | RED_HERO_CLIENT = 0x0101, 40 | RED_ENGINEER_CLIENT = 0x0102, 41 | RED_INFANTRY_III_CLIENT = 0x0103, 42 | RED_INFANTRY_IV_CLIENT = 0x0104, 43 | RED_INFANTRY_V_CLIENT = 0x0105, 44 | RED_AERIAL_CLIENT = 0x0106, 45 | 46 | BLUE_HERO_CLIENT = 0x0165, 47 | BLUE_ENGINEER_CLIENT = 0x0166, 48 | BLUE_INFANTRY_III_CLIENT = 0x0167, 49 | BLUE_INFANTRY_IV_CLIENT = 0x0168, 50 | BLUE_INFANTRY_V_CLIENT = 0x0169, 51 | BLUE_AERIAL_CLIENT = 0x016A, 52 | 53 | REFEREE_SERVER = 0x8080, 54 | }; 55 | 56 | constexpr FullRobotId() 57 | : value_{UNKNOWN} {} 58 | constexpr explicit FullRobotId(uint16_t value) 59 | : value_{static_cast(value)} {} 60 | constexpr FullRobotId(Value value) // NOLINT(google-explicit-constructor) 61 | : value_{value} {} 62 | constexpr FullRobotId(RobotId::Value value) // NOLINT(google-explicit-constructor) 63 | : value_{static_cast(static_cast(value))} {} 64 | 65 | explicit constexpr operator uint16_t() const { return value_; } 66 | constexpr operator Value() const { return value_; } // NOLINT(google-explicit-constructor) 67 | 68 | constexpr FullRobotId& operator=(Value value) { 69 | value_ = value; 70 | return *this; 71 | } 72 | constexpr bool operator==(const Value value) const { return value_ == value; } 73 | constexpr bool operator!=(const Value value) const { return value_ != value; } 74 | 75 | constexpr RobotColor color() const { 76 | if (value_ == REFEREE_SERVER) [[unlikely]] 77 | return RobotColor::UNKNOWN; 78 | 79 | return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; 80 | } 81 | 82 | constexpr FullRobotId robot() const { 83 | if (value_ == REFEREE_SERVER) [[unlikely]] 84 | return FullRobotId::UNKNOWN; 85 | 86 | return static_cast(value_ & 0x0100); 87 | } 88 | 89 | constexpr FullRobotId client() const { 90 | if (value_ == REFEREE_SERVER) [[unlikely]] 91 | return FullRobotId::UNKNOWN; 92 | 93 | return static_cast(value_ | 0x0100); 94 | } 95 | 96 | private: 97 | Value value_; 98 | }; 99 | 100 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_msgs { 6 | 7 | enum class GameStage : uint8_t { 8 | NOT_START = 0, 9 | PREPARATION = 1, 10 | REFEREE_CHECK = 2, 11 | COUNTDOWN = 3, 12 | STARTED = 4, 13 | SETTLING = 5, 14 | UNKNOWN = UINT8_MAX, 15 | }; 16 | 17 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/keyboard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_msgs { 7 | 8 | struct __attribute__((packed)) Keyboard { 9 | constexpr static inline Keyboard zero() { 10 | constexpr uint16_t zero = 0; 11 | return std::bit_cast(zero); 12 | } 13 | 14 | bool w : 1; 15 | bool s : 1; 16 | bool a : 1; 17 | bool d : 1; 18 | bool shift : 1; 19 | bool ctrl : 1; 20 | bool q : 1; 21 | bool e : 1; 22 | bool r : 1; 23 | bool f : 1; 24 | bool g : 1; 25 | bool z : 1; 26 | bool x : 1; 27 | bool c : 1; 28 | bool v : 1; 29 | bool b : 1; 30 | }; 31 | 32 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/mouse.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_msgs { 7 | 8 | struct __attribute__((packed)) Mouse { 9 | constexpr static inline Mouse zero() { 10 | constexpr uint8_t zero = 0; 11 | return std::bit_cast(zero); 12 | } 13 | 14 | bool left : 1; 15 | bool right : 1; 16 | }; 17 | 18 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_color.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_msgs { 6 | 7 | enum class RobotColor : uint8_t { 8 | UNKNOWN = 0, 9 | RED = 1, 10 | BLUE = 2, 11 | }; 12 | 13 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robot_id.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "robot_color.hpp" 6 | 7 | namespace rmcs_msgs { 8 | enum class ArmorID : uint16_t { 9 | Unknown = 0, 10 | Hero = 1, 11 | Engineer = 2, 12 | InfantryIII = 3, 13 | InfantryIV = 4, 14 | InfantryV = 5, 15 | Aerial = 6, 16 | Sentry = 7, 17 | Dart = 8, 18 | Radar = 9, 19 | Outpost = 10, 20 | Base = 11, 21 | }; 22 | 23 | class RobotId { 24 | public: 25 | enum Value : uint8_t { 26 | UNKNOWN = 0, 27 | 28 | RED_HERO = 1, 29 | RED_ENGINEER = 2, 30 | RED_INFANTRY_III = 3, 31 | RED_INFANTRY_IV = 4, 32 | RED_INFANTRY_V = 5, 33 | RED_AERIAL = 6, 34 | RED_SENTRY = 7, 35 | RED_DART = 8, 36 | RED_RADAR = 9, 37 | RED_OUTPOST = 10, 38 | RED_BASE = 11, 39 | 40 | BLUE_HERO = 101, 41 | BLUE_ENGINEER = 102, 42 | BLUE_INFANTRY_III = 103, 43 | BLUE_INFANTRY_IV = 104, 44 | BLUE_INFANTRY_V = 105, 45 | BLUE_AERIAL = 106, 46 | BLUE_SENTRY = 107, 47 | BLUE_DART = 108, 48 | BLUE_RADAR = 109, 49 | BLUE_OUTPOST = 110, 50 | BLUE_BASE = 111, 51 | }; 52 | 53 | constexpr RobotId() 54 | : value_{UNKNOWN} {} 55 | constexpr explicit RobotId(uint8_t value) 56 | : value_{static_cast(value)} {} 57 | constexpr RobotId(Value value) // NOLINT(google-explicit-constructor) 58 | : value_{value} {} 59 | 60 | explicit constexpr operator uint8_t() const { return value_; } 61 | constexpr operator Value() const { return value_; } // NOLINT(google-explicit-constructor) 62 | 63 | constexpr RobotId& operator=(Value value) { 64 | value_ = value; 65 | return *this; 66 | } 67 | constexpr bool operator==(const Value value) const { return value_ == value; } 68 | constexpr bool operator!=(const Value value) const { return value_ != value; } 69 | 70 | constexpr RobotColor color() const { return value_ & 0x40 ? RobotColor::BLUE : RobotColor::RED; } 71 | 72 | constexpr ArmorID id() const { 73 | return value_ > 100 ? static_cast(value_ - 100) : static_cast(value_); 74 | } 75 | 76 | private: 77 | Value value_; 78 | }; 79 | 80 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_msgs { 7 | 8 | struct SerialInterface { 9 | std::function read; 10 | std::function write; 11 | }; 12 | 13 | } -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_mode.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_msgs { 6 | 7 | enum class ShootMode : uint8_t { 8 | SINGLE = 0, // Fires one projectile each time the mouse is clicked. 9 | PRECISE = 1, // Fires one projectile each time the mouse is clicked, with improved accuracy at 10 | // the cost of increased firing delay. 11 | AUTOMATIC = 2, // Continuously fires projectiles while the mouse is held down. 12 | OVERDRIVE = 3, // Continuously fires projectiles while the mouse is held down, ignoring heat 13 | // limits but causing health loss. 14 | }; 15 | 16 | } -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/shoot_status.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace rmcs_msgs { 4 | 5 | struct ShootStatus { 6 | bool ready; 7 | int fired_count; 8 | int jammed_count; 9 | int dry_fed_count; 10 | int single_shot_cancelled_count; 11 | }; 12 | 13 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/switch.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_msgs { 6 | 7 | enum class Switch : uint8_t { UNKNOWN = 0, UP = 1, DOWN = 2, MIDDLE = 3 }; 8 | 9 | } // namespace rmcs_msgs -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/ArmorPlate.msg: -------------------------------------------------------------------------------- 1 | uint16 id 2 | 3 | Point2f left_top 4 | Point2f right_top 5 | Point2f left_bottom 6 | Point2f right_bottom -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/ArmorPlateArray.msg: -------------------------------------------------------------------------------- 1 | ArmorPlate[] armor_plate_array -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/GameStatus.msg: -------------------------------------------------------------------------------- 1 | RobotStatus friends_hero 2 | RobotStatus friends_engineer 3 | RobotStatus friends_infantry_iii 4 | RobotStatus friends_infantry_iv 5 | RobotStatus friends_infantry_v 6 | RobotStatus friends_sentry 7 | 8 | RobotStatus enemies_hero 9 | RobotStatus enemies_engineer 10 | RobotStatus enemies_infantry_iii 11 | RobotStatus enemies_infantry_iv 12 | RobotStatus enemies_infantry_v 13 | RobotStatus enemies_sentry 14 | 15 | int32 friends_outpost_hp 16 | int32 enemies_outpost_hp 17 | 18 | int32 friends_base_hp 19 | int32 enemies_base_hp 20 | 21 | int32 bullet 22 | 23 | uint32 rfid -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/Point2f.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/RobotPose.msg: -------------------------------------------------------------------------------- 1 | int64 id 2 | geometry_msgs/Pose pose 3 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/msg/RobotStatus.msg: -------------------------------------------------------------------------------- 1 | geometry_msgs/Pose2D pose 2 | uint16 hp -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcs_msgs 5 | 0.0.0 6 | TODO: Package description 7 | Qzh 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rosidl_interface_packages 13 | 14 | rclcpp 15 | std_msgs 16 | geometry_msgs 17 | builtin_interfaces 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | project(rmcs_utility) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 5 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") 6 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-O2 -Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | find_package (ament_cmake_auto REQUIRED) 13 | ament_auto_find_build_dependencies () 14 | 15 | include_directories(${PROJECT_SOURCE_DIR}/include) 16 | 17 | ament_auto_package() -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rmcs_utility::dji_crc { 7 | namespace internal { 8 | 9 | template 10 | inline auto& get_tail(T& data) { 11 | return *reinterpret_cast(reinterpret_cast(&data) + sizeof(T) - sizeof(uint8_t)); 12 | } 13 | 14 | constexpr uint8_t crc8_init = 0xff; 15 | constexpr uint8_t crc8_table[256] = { 16 | 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 17 | 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 18 | 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 19 | 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 20 | 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 21 | 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, 22 | 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 23 | 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 24 | 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 25 | 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 26 | 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 27 | 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, 28 | 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 29 | 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 30 | 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 31 | 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, 32 | }; 33 | 34 | constexpr uint16_t crc16_init = 0xffff; 35 | constexpr uint16_t crc16_table[256] = { 36 | 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 37 | 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 38 | 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, 39 | 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, 40 | 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 41 | 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, 42 | 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, 43 | 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 44 | 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 45 | 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, 46 | 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 47 | 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, 48 | 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 49 | 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 50 | 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, 51 | 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, 52 | 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 53 | 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, 54 | 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595, 55 | 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 56 | 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 57 | 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; 58 | 59 | } // namespace internal 60 | 61 | inline uint8_t calculate_crc8(const void* data, size_t length) { 62 | auto* p = reinterpret_cast(data); 63 | auto result = internal::crc8_init; 64 | while (length--) 65 | result = internal::crc8_table[result ^ (*p++)]; 66 | return result; 67 | } 68 | 69 | inline bool verify_crc8(const void* data, size_t length) { 70 | auto checksum = calculate_crc8(data, length - sizeof(uint8_t)); 71 | return checksum == *(reinterpret_cast(data) + length - sizeof(uint8_t)); 72 | } 73 | template 74 | inline bool verify_crc8(const T& package) { 75 | static_assert(sizeof(T) > sizeof(uint8_t)); 76 | return verify_crc8(&package, sizeof(package)); 77 | } 78 | 79 | inline void append_crc8(void* data, size_t length) { 80 | auto checksum = calculate_crc8(data, length - sizeof(uint8_t)); 81 | *(reinterpret_cast(data) + length - sizeof(uint8_t)) = checksum; 82 | } 83 | template 84 | inline void append_crc8(T& package) { 85 | static_assert(sizeof(T) > sizeof(uint8_t)); 86 | append_crc8(&package, sizeof(package)); 87 | } 88 | 89 | inline uint16_t calculate_crc16(const void* data, size_t length) { 90 | auto* p = reinterpret_cast(data); 91 | uint16_t result = internal::crc16_init; 92 | while (length--) { 93 | result = (result >> 8) ^ internal::crc16_table[(result ^ (*p++)) & 0x00ff]; 94 | } 95 | return result; 96 | } 97 | 98 | inline bool verify_crc16(const void* data, size_t length) { 99 | auto checksum = calculate_crc16(data, length - sizeof(uint16_t)); 100 | return checksum 101 | == *reinterpret_cast( 102 | reinterpret_cast(data) + length - sizeof(uint16_t)); 103 | } 104 | template 105 | inline bool verify_crc16(const T& package) { 106 | static_assert(sizeof(T) > sizeof(uint16_t)); 107 | return verify_crc16(&package, sizeof(package)); 108 | } 109 | 110 | inline void append_crc16(void* data, size_t length) { 111 | auto checksum = calculate_crc16(data, length - sizeof(uint16_t)); 112 | *reinterpret_cast(reinterpret_cast(data) + length - sizeof(uint16_t)) = 113 | checksum; 114 | } 115 | template 116 | inline void append_crc16(T& package) { 117 | static_assert(sizeof(T) > sizeof(uint16_t)); 118 | append_crc16(&package, sizeof(package)); 119 | } 120 | 121 | } // namespace rmcs_utility::dji_crc -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace rmcs_utility { 11 | 12 | /// @brief Implements a reliable, lock-free, arbitrary-length cross-thread buffer. 13 | /// @tparam T The type of data to be stored in the buffer. 14 | /// @tparam i_am_very_sure_this_is_trivially_copyable_and_destructible A flag to bypass type checks. 15 | /// Set to `true` only if you are certain that `T` is trivially copyable and destructible. 16 | template 17 | requires(std::is_trivially_copyable_v && std::is_trivially_destructible_v) 18 | || i_am_very_sure_this_is_trivially_copyable_and_destructible class DoubleBuffer { 19 | public: 20 | /// @brief Writes data to the buffer in a thread-safe manner. 21 | /// @param data The data to be written to the buffer. 22 | void write(const T& data) noexcept { 23 | const uint64_t current = current_.load(std::memory_order_relaxed); 24 | const uint64_t next = current + 1; 25 | std::memcpy(&buffers_[next & 1], &data, sizeof(T)); 26 | current_.store(next, std::memory_order_release); 27 | } 28 | 29 | /// @brief Reads data from the buffer in a thread-safe manner. 30 | /// @param data The variable where the read data will be stored. 31 | /// @return Returns `true` if the read was successful, `false` otherwise. 32 | /// @note Under high write pressure, consumers may occasionally read incorrect data. 33 | /// The function explicitly returns `false` to signal failure. Readers can choose to retry 34 | /// (e.g., using a `while` loop) or cancel the operation. 35 | /// @note If the function returns `false`, the variable `data` will be filled with invalid data. 36 | bool read(T& data) noexcept { 37 | const uint64_t current = current_.load(std::memory_order_acquire); 38 | std::memcpy(&data, &buffers_[current & 1], sizeof(T)); 39 | return current_.load(std::memory_order_acquire) == current; 40 | } 41 | 42 | private: 43 | /// @brief Internal buffer structure to hold the data. 44 | /// @note The buffer alignment is set to the maximum of 64 bytes and `T`'s natural alignment. 45 | /// This serves two purposes: 46 | /// 1. Prevents false sharing by ensuring 64-byte cache-line separation. 47 | /// 2. Guarantees proper alignment for `T` type operations. 48 | struct alignas(std::max(size_t{64}, alignof(T))) Buffer { 49 | std::byte data[sizeof(T)]; 50 | }; 51 | 52 | alignas(64) std::atomic current_{0}; 53 | alignas(64) Buffer buffers_[2]; 54 | }; 55 | 56 | } // namespace rmcs_utility -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rmcs_utility { 6 | 7 | class FpsCounter { 8 | public: 9 | explicit FpsCounter( 10 | std::chrono::steady_clock::duration measurement_window = std::chrono::seconds(1)) 11 | : measurement_window_(measurement_window) 12 | , start_(std::chrono::steady_clock::now()) {} 13 | 14 | bool count() { 15 | ++count_; 16 | 17 | auto now = std::chrono::steady_clock::now(); 18 | auto elapsed_time = now - start_; 19 | if (elapsed_time >= measurement_window_) { 20 | start_ = now; 21 | fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); 22 | count_ = 0; 23 | return true; 24 | } 25 | 26 | return false; 27 | } 28 | 29 | double fps() const { return fps_; } 30 | 31 | private: 32 | std::chrono::steady_clock::duration measurement_window_; 33 | std::chrono::steady_clock::time_point start_; 34 | 35 | int64_t count_ = 0; 36 | double fps_ = 0; 37 | }; 38 | 39 | } // namespace rmcs_utility -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | namespace rmcs_utility { 11 | 12 | enum class ReceiveResult : uint8_t { 13 | SUCCESS = 0, 14 | TIMEOUT = 1, 15 | HEADER_INVALID = 2, 16 | VERIFY_INVALID = 3 17 | }; 18 | 19 | template 20 | concept is_byte = 21 | std::is_same_v || std::is_same_v || std::is_same_v; 22 | 23 | template 24 | concept is_readable_stream = requires(SerialT& serial, ByteT* pointer, size_t size) { 25 | { serial.read(pointer, size) } -> std::convertible_to; 26 | }; 27 | 28 | template 29 | concept is_verify_function = requires(const F& f, const PackageT& package) { 30 | { f(package) } -> std::convertible_to; 31 | }; 32 | 33 | template 34 | requires std::is_trivially_copyable_v inline auto receive_package( 35 | is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, 36 | const is_verify_function auto& header_verify, 37 | const is_verify_function auto& verify) -> ReceiveResult { 38 | if (cache_size == sizeof(PackageT)) 39 | return ReceiveResult::SUCCESS; 40 | 41 | auto* buffer_pointer = reinterpret_cast(&buffer); 42 | cache_size += stream.read(buffer_pointer + cache_size, sizeof(PackageT) - cache_size); 43 | 44 | if (cache_size == 0 || cache_size < header_size) 45 | return ReceiveResult::TIMEOUT; 46 | 47 | ReceiveResult result; 48 | if (header_size == 0 || header_verify(buffer)) { 49 | if (cache_size != sizeof(PackageT)) 50 | return ReceiveResult::TIMEOUT; 51 | if (verify(buffer)) 52 | return ReceiveResult::SUCCESS; 53 | else 54 | result = ReceiveResult::VERIFY_INVALID; 55 | } else { 56 | result = ReceiveResult::HEADER_INVALID; 57 | } 58 | 59 | while (true) { 60 | memmove(buffer_pointer, buffer_pointer + 1, --cache_size); 61 | if (cache_size == 0 || cache_size < header_size || header_size == 0 62 | || header_verify(*std::launder(reinterpret_cast(buffer_pointer)))) { 63 | break; 64 | } 65 | } 66 | return result; 67 | } 68 | 69 | template 70 | requires std::is_trivially_copyable_v inline auto receive_package( 71 | is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, HeaderT header, 72 | const is_verify_function auto& verify) -> ReceiveResult { 73 | return receive_package( 74 | stream, buffer, cache_size, 75 | [header](const PackageT& package) { 76 | HeaderT actual_header; 77 | std::memcpy(&actual_header, &package, sizeof(HeaderT)); 78 | return actual_header == header; 79 | }, 80 | verify); 81 | } 82 | 83 | template 84 | requires std::is_trivially_copyable_v inline auto receive_package( 85 | is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, 86 | const is_verify_function auto& verify) -> ReceiveResult { 87 | return receive_package<0, std::byte>( 88 | stream, buffer, cache_size, [](const PackageT&) { return true; }, verify); 89 | } 90 | 91 | } // namespace rmcs_utility -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rmcs_utility { 10 | 11 | template 12 | class RingBuffer { 13 | public: 14 | constexpr explicit RingBuffer(size_t size) { 15 | if (size <= 2) 16 | size = 2; 17 | else 18 | size = round_up_to_next_power_of_2(size); 19 | mask = size - 1; 20 | storage_ = new Storage[size]; 21 | }; 22 | 23 | ~RingBuffer() { 24 | clear(); 25 | delete[] storage_; 26 | } 27 | 28 | size_t max_size() const { return mask + 1; } 29 | 30 | /*! 31 | * \brief Check how many elements can be read from the buffer 32 | * \return Number of elements that can be read 33 | */ 34 | size_t readable() const { 35 | return in_.load(std::memory_order::acquire) - out_.load(std::memory_order::relaxed); 36 | } 37 | 38 | /*! 39 | * \brief Check how many elements can be written into the buffer 40 | * \return Number of free slots that can be be written 41 | */ 42 | size_t writeable() const { 43 | return max_size() 44 | - (in_.load(std::memory_order::relaxed) - out_.load(std::memory_order::acquire)); 45 | } 46 | 47 | /*! 48 | * \brief Gets the first element in the buffer on consumed side 49 | * 50 | * It is safe to use and modify item contents only on consumer side 51 | * 52 | * \return Pointer to first element, nullptr if buffer was empty 53 | */ 54 | T* front() { 55 | auto out = out_.load(std::memory_order::relaxed); 56 | 57 | if (out == in_.load(std::memory_order::acquire)) 58 | return nullptr; 59 | else 60 | return std::launder(reinterpret_cast(storage_[out & mask].data)); 61 | } 62 | 63 | /*! 64 | * \brief Gets the last element in the buffer on consumed side 65 | * 66 | * It is safe to use and modify item contents only on consumer side 67 | * 68 | * \return Pointer to last element, nullptr if buffer was empty 69 | */ 70 | T* back() { 71 | auto in = in_.load(std::memory_order::acquire); 72 | 73 | if (in == out_.load(std::memory_order::relaxed)) 74 | return nullptr; 75 | else 76 | return std::launder(reinterpret_cast(storage_[in & mask].data)); 77 | } 78 | 79 | template 80 | requires requires(F f, std::byte* storage) { f(storage); } 81 | size_t emplace_back_multi(F construct_functor, size_t count = -1) { 82 | auto in = in_.load(std::memory_order::relaxed); 83 | auto out = out_.load(std::memory_order::acquire); 84 | 85 | auto writeable = max_size() - (in - out); 86 | 87 | if (count > writeable) 88 | count = writeable; 89 | if (!count) 90 | return 0; 91 | 92 | auto offset = in & mask; 93 | auto slice = std::min(count, max_size() - offset); 94 | 95 | for (size_t i = 0; i < slice; i++) 96 | construct_functor(storage_[offset + i].data); 97 | for (size_t i = 0; i < count - slice; i++) 98 | construct_functor(storage_[i].data); 99 | 100 | std::atomic_signal_fence(std::memory_order::release); 101 | in_.store(in + count, std::memory_order::release); 102 | 103 | return count; 104 | } 105 | 106 | template 107 | bool emplace_back(Args&&... args) { 108 | return emplace_back_multi( 109 | [&](std::byte* storage) { new (storage) T{std::forward(args...)}; }, 1); 110 | } 111 | 112 | template 113 | requires requires(F f) { T{f()}; } size_t push_back_multi(F generator, size_t count = -1) { 114 | return emplace_back_multi([&](std::byte* storage) { new (storage) T{generator()}; }, count); 115 | } 116 | 117 | bool push_back(const T& value) { 118 | return emplace_back_multi([&](std::byte* storage) { new (storage) T{value}; }, 1); 119 | } 120 | bool push_back(T&& value) { 121 | return emplace_back_multi( 122 | [&](std::byte* storage) { new (storage) T{std::move(value)}; }, 1); 123 | } 124 | 125 | template 126 | requires requires(F f, T t) { f(std::move(t)); } 127 | size_t pop_front_multi(F callback_functor, size_t count = -1) { 128 | auto in = in_.load(std::memory_order::acquire); 129 | auto out = out_.load(std::memory_order::relaxed); 130 | 131 | auto readable = in - out; 132 | if (count > readable) 133 | count = readable; 134 | if (!count) 135 | return 0; 136 | 137 | auto offset = out & mask; 138 | auto slice = std::min(count, max_size() - offset); 139 | 140 | auto process = [&callback_functor](std::byte* storage) { 141 | auto& element = *std::launder(reinterpret_cast(storage)); 142 | callback_functor(std::move(element)); 143 | std::destroy_at(&element); 144 | }; 145 | for (size_t i = 0; i < slice; i++) 146 | process(storage_[offset + i].data); 147 | for (size_t i = 0; i < count - slice; i++) 148 | process(storage_[i].data); 149 | 150 | std::atomic_signal_fence(std::memory_order::release); 151 | out_.store(out + count, std::memory_order::release); 152 | 153 | return count; 154 | } 155 | 156 | template 157 | requires requires(F f, T t) { f(std::move(t)); } bool pop_front(F&& callback_functor) { 158 | return pop_front_multi(std::forward(callback_functor), 1); 159 | } 160 | 161 | /*! 162 | * \brief Clear buffer 163 | * \return Number of elements that be erased 164 | */ 165 | size_t clear() { 166 | return pop_front_multi([](T&&) {}); 167 | } 168 | 169 | private: 170 | constexpr static size_t round_up_to_next_power_of_2(size_t n) { 171 | n--; 172 | n |= n >> 1; 173 | n |= n >> 2; 174 | n |= n >> 4; 175 | n |= n >> 8; 176 | n |= n >> 16; 177 | n |= n >> 32; 178 | n++; 179 | return n; 180 | } 181 | 182 | size_t mask; 183 | 184 | std::atomic in_{0}, out_{0}; 185 | struct Storage { 186 | alignas(T) std::byte data[sizeof(T)]; 187 | }* storage_; 188 | }; 189 | 190 | }; // namespace rmcs_utility 191 | -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/include/rmcs_utility/tick_timer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace rmcs_utility { 4 | 5 | class TickTimer { 6 | public: 7 | void reset(unsigned int cooldown) { counter_ = 2 * cooldown; } 8 | 9 | bool tick() { 10 | if (counter_ == 0) [[unlikely]] { 11 | counter_ = 1; 12 | return true; 13 | } else { 14 | counter_ -= 2; 15 | return false; 16 | } 17 | } 18 | 19 | private: 20 | unsigned int counter_ = 1; 21 | }; 22 | 23 | } // namespace rmcs_utility -------------------------------------------------------------------------------- /rmcs_ws/src/rmcs_utility/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcs_utility 5 | 1.0.0 6 | Utility tool for serial port reception of data packets 7 | Qzh 8 | GPL-3.0-only 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | --------------------------------------------------------------------------------