diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4fcde4e1..42f5b137 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -17,10 +17,28 @@ } ], "containerEnv": { - "DISPLAY": ":1" + "DISPLAY": "${localEnv:DISPLAY}" }, "runArgs": [ "--network", "host" - ] + ], + "customizations": { + "vscode": { + "extensions": [ + // C++ language support + "llvm-vs-code-extensions.vscode-clangd", + // Python language support + "ms-python.vscode-pylance", + "ms-python.python", + "ms-python.debugpy", + // CMake language support + "twxs.cmake", + // Code spell checking + "streetsidesoftware.code-spell-checker", + // Git enhancements + "mhutchie.git-graph" + ] + } + } } \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index afd07cb5..171323b4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,16 +1,24 @@ -[submodule "rmcs_ws/src/serial"] - path = rmcs_ws/src/serial - url = https://github.com/Alliance-Algorithm/ros2-serial.git - branch = ros2 +[submodule "rmcs_ws/src/rmcs_core/librmcs"] + path = rmcs_ws/src/rmcs_core/librmcs + url = https://github.com/Alliance-Algorithm/librmcs.git +[submodule "rmcs_ws/src/fast_tf"] + path = rmcs_ws/src/fast_tf + url = https://github.com/qzhhhi/FastTF.git [submodule "rmcs_ws/src/hikcamera"] path = rmcs_ws/src/hikcamera url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git [submodule "rmcs_ws/src/fast_tf"] path = rmcs_ws/src/fast_tf url = https://github.com/qzhhhi/FastTF.git -[submodule "rmcs_ws/src/rmcs_referee"] - path = rmcs_ws/src/rmcs_referee - url = https://github.com/Alliance-Algorithm/rmcs_referee -[submodule "rmcs_ws/src/rmcs_auto_aim"] - path = rmcs_ws/src/rmcs_auto_aim - url = https://github.com/Alliance-Algorithm/rmcs_auto_aim.git +[submodule "rmcs_ws/src/rmcs_core/include/librmcs"] + path = rmcs_ws/src/rmcs_core/include/librmcs + url = git@github.com:Alliance-Algorithm/librmcs.git +[submodule "rmcs_ws/src/rmcs_core/include/rmcs_core/librmcs"] + path = rmcs_ws/src/rmcs_core/include/rmcs_core/librmcs + url = git@github.com:Alliance-Algorithm/librmcs.git +[submodule "rmcs_ws/src/rmcs_core/librmcs"] + path = rmcs_ws/src/rmcs_core/librmcs + url = git@github.com:Alliance-Algorithm/librmcs.git +[submodule "rmcs_ws/src/serial"] + path = rmcs_ws/src/serial + url = https://github.com/Alliance-Algorithm/ros2-serial.git diff --git a/.script/attach-remote b/.script/attach-remote new file mode 100755 index 00000000..b249b943 --- /dev/null +++ b/.script/attach-remote @@ -0,0 +1,8 @@ +#! /bin/bash + +# if arg[1] == "-r" +if [ "$1" == "-r" ]; then + ssh-remote "service rmcs restart && service rmcs attach" +else + ssh-remote "service rmcs attach" +fi diff --git a/.script/build-rmcs b/.script/build-rmcs new file mode 100755 index 00000000..ef5d85ff --- /dev/null +++ b/.script/build-rmcs @@ -0,0 +1,7 @@ +#! /bin/bash + +source /opt/ros/jazzy/setup.bash + +cd ${RMCS_PATH}/rmcs_ws + +colcon build --symlink-install --merge-install $@ diff --git a/.script/clean-rmcs b/.script/clean-rmcs new file mode 100755 index 00000000..be24a47d --- /dev/null +++ b/.script/clean-rmcs @@ -0,0 +1,3 @@ +#! /bin/bash + +rm -rf ${RMCS_PATH}/rmcs_ws/build ${RMCS_PATH}/rmcs_ws/install ${RMCS_PATH}/rmcs_ws/log diff --git a/.script/complete/_set-remote b/.script/complete/_set-remote new file mode 100644 index 00000000..008a9f87 --- /dev/null +++ b/.script/complete/_set-remote @@ -0,0 +1,22 @@ +#compdef set-remote + +_remote_hosts() { + local hosts=( + "169.254.233.233" + "alliance-sentry.local" + "alliance-infantry.local" + "alliance-hero.local" + ) + + # 从 ~/.ssh/config 提取最近使用的 HostName + if [[ -f ~/.ssh/config ]]; then + local extracted + extracted=(${(f)"$(grep -A1 'Host remote' ~/.ssh/config | grep HostName | awk '{print $2}')"}) + hosts+=(${extracted}) + fi + + compadd -- $hosts +} + +_arguments \ + '1:远程主机地址:_remote_hosts' \ No newline at end of file diff --git a/.script/launch-rmcs b/.script/launch-rmcs new file mode 100755 index 00000000..c9848811 --- /dev/null +++ b/.script/launch-rmcs @@ -0,0 +1,10 @@ +#! /bin/bash + +source ~/env_setup.bash + +if [ -z "${RMCS_ROBOT_TYPE}" ]; then + echo "Error: Please set robot type first (e.g. 'set-robot sentry')." + exit 1 +fi + +ros2 launch rmcs_bringup rmcs.launch.py robot:=${RMCS_ROBOT_TYPE} diff --git a/.script/service-remote.sh b/.script/service-remote.sh deleted file mode 100755 index 74c94c17..00000000 --- a/.script/service-remote.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -/bin/ssh remote "service rmcs $1" \ No newline at end of file diff --git a/.script/service.sh b/.script/service.sh deleted file mode 100755 index a61306bd..00000000 --- a/.script/service.sh +++ /dev/null @@ -1,48 +0,0 @@ -#! /bin/bash - -set -e - -start() { - if [[ -z "$HOME" ]]; then - export HOME=/root - fi - source /opt/ros/humble/setup.bash - source /rmcs_install/setup.bash - nohup ros2 launch rmcs_bringup rmcs.launch.py &> rmcs.launch.out 2>&1 & echo $! > /tmp/rmcs.pid -} - -stop() { - if [ -f "/tmp/rmcs.pid" ]; then - pkill -P "$(cat /tmp/rmcs.pid)" - fi -} - -check_running() { - if [ -f "/tmp/rmcs.pid" ]; then - if ps -p "$(cat /tmp/rmcs.pid)" > /dev/null - then - echo 'Fatal: RMCS daemon is still running!' - exit 1 - fi - fi -} - -case "$1" in - start) - check_running - start - ;; - stop) - stop - ;; - restart) - stop - start - ;; - *) - echo 'Usage: /etc/init.d/rmcs {start|stop|restart}' - exit 1 - ;; -esac - -exit 0 \ No newline at end of file diff --git a/.script/set-remote b/.script/set-remote new file mode 100755 index 00000000..860ca2c0 --- /dev/null +++ b/.script/set-remote @@ -0,0 +1,257 @@ +#!/bin/python3 + +import sys +import re +import os + +from colorama import Fore, Style + + +def main(): + if len(sys.argv) <= 1 or len(sys.argv) > 2 or sys.argv[1] in ("-h", "--help"): + print("Usage: set-remote ") + print(" 1. IPv4 / IPv6 address (e.g., 169.254.233.233)") + print(" 2. IPv4 / IPv6 link-local address (e.g., fe80::c6d0:e3ff:fed7:ed12%eth0)") + print(" 3. mDNS hostname (e.g., my-sentry.local)") + sys.exit(1) + + host_remote = sys.argv[1] + if host_remote.endswith(".local") or host_remote.endswith(".local."): + print("An mDNS hostname was provided. Will resolve it to an IP address.") + host_remote = mdns_lookup(host_remote) + + set_remote(host_remote) + print( + f"Successfully set remote host to {Fore.LIGHTGREEN_EX}{host_remote}{Style.RESET_ALL}." + ) + + +def mdns_lookup(hostname): + import dpkt + import psutil + import select + import socket + import struct + + mdns_udp_port = 5353 + + dns_query = dpkt.dns.DNS( + b"\x00\x00\x01\x00\x00\x01\x00\x00" + b"\x00\x00\x00\x00\x00\x00\x01\x00\x01" + ) + dns_query.qd[0].name = hostname + dns_query.qd[0].type = dpkt.dns.DNS_A + dns_query_packet4 = dns_query.pack() + dns_query.qd[0].type = dpkt.dns.DNS_AAAA + dns_query_packet6 = dns_query.pack() + + socks = [] + sock_info = {} + + print("Sending mDNS query packet to interfaces...") + for interface, addresses in psutil.net_if_addrs().items(): + print(f" - {interface}: ", end="") + if interface.startswith("lo") or interface.startswith("loopback"): + print(f"{Fore.LIGHTBLACK_EX}Skipped (loopback interface).{Style.RESET_ALL}") + continue + elif interface.startswith("docker") or interface.startswith("br-"): + print(f"{Fore.LIGHTBLACK_EX}Skipped (docker interface).{Style.RESET_ALL}") + continue + + interface_ip4 = None + interface_ip6 = None + + for addr in addresses: + if addr.family == socket.AF_INET: + interface_ip4 = addr.address + elif addr.family == socket.AF_INET6: + if addr.address.startswith("fe80"): + interface_ip6 = addr.address + + if interface_ip4 is not None: + multicast_group4 = "224.0.0.251" # IPv4 mDNS multicast group + + sock4 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock4.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock4.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + sock4.bind(("0.0.0.0", mdns_udp_port)) + + # Join the IPv4 multicast group with the given interface + sock4.setsockopt( + socket.IPPROTO_IP, + socket.IP_ADD_MEMBERSHIP, + struct.pack( + "4s4s", + socket.inet_aton(multicast_group4), + socket.inet_aton(interface_ip4), + ), + ) + # Set the receiving interface + sock4.setsockopt(socket.SOL_SOCKET, 25, interface.encode("utf-8") + b"\0") + # Set the sending interface + sock4.setsockopt( + socket.IPPROTO_IP, + socket.IP_MULTICAST_IF, + socket.inet_aton(interface_ip4), + ) + + sock4.sendto(dns_query_packet4, (multicast_group4, mdns_udp_port)) + sock4.sendto(dns_query_packet6, (multicast_group4, mdns_udp_port)) + print( + f"{Fore.LIGHTGREEN_EX}Query sent successfully (IPv4).{Style.RESET_ALL}" + ) + + sock_info[sock4] = (interface, "IPv4") + socks.append(sock4) + elif interface_ip6 is not None: + multicast_group6 = "ff02::fb" # IPv6 mDNS multicast group + + sock6 = socket.socket(socket.AF_INET6, socket.SOCK_DGRAM) + sock6.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock6.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1) + sock6.bind(("::", mdns_udp_port)) + + interface_index = socket.if_nametoindex(interface) + # Join the IPv6 multicast group with the given interface + sock6.setsockopt( + socket.IPPROTO_IPV6, + socket.IPV6_JOIN_GROUP, + struct.pack( + "16si", + socket.inet_pton(socket.AF_INET6, multicast_group6), + interface_index, + ), + ) + # Set the receiving interface + sock6.setsockopt(socket.SOL_SOCKET, 25, interface.encode("utf-8") + b"\0") + # Set the sending interface + sock6.setsockopt( + socket.IPPROTO_IPV6, + socket.IPV6_MULTICAST_IF, + interface_index, + ) + + sock6.sendto(dns_query_packet6, (multicast_group6, mdns_udp_port)) + print( + f"{Fore.LIGHTGREEN_EX}Query sent successfully (IPv6).{Style.RESET_ALL}" + ) + + sock_info[sock6] = (interface, "IPv6") + socks.append(sock6) + else: + print(f"{Fore.LIGHTBLACK_EX}Skipped (interface down).{Style.RESET_ALL}") + + discovered_list = [] + + while True: + ready_socks, _, _ = select.select(socks, [], [], 1.0) + + if not ready_socks: + index = input( + "Enter the index to select a address, or press Enter to search again: " + if discovered_list + else "No addresses found. Press Enter to search again or Ctrl+C to exit: " + ) + if index: + index = int(index) + _, ip = discovered_list[index] + return ip + else: + print("\033[A\r\033[KRe-sending mDNS query packet...") + for sock in socks: + interface, ip_version = sock_info[sock] + if ip_version == "IPv4": + sock.sendto( + dns_query_packet4, (multicast_group4, mdns_udp_port) + ) + sock.sendto( + dns_query_packet6, (multicast_group4, mdns_udp_port) + ) + elif ip_version == "IPv6": + sock.sendto( + dns_query_packet6, (multicast_group6, mdns_udp_port) + ) + print( + f" - {interface}: " + f"{Fore.LIGHTGREEN_EX}Query re-sent successfully ({ip_version}).{Style.RESET_ALL}" + ) + + continue + + for sock in ready_socks: + m = sock.recvfrom(1024) + + try: + dns = dpkt.dns.DNS(m[0]) + except dpkt.UnpackError: + continue + + if len(dns.an) == 0: + continue + + host = dns.an[0].name + if host != hostname: + continue + if dns.an[0].type == dpkt.dns.DNS_A: + ip = socket.inet_ntoa(dns.an[0].rdata) + elif dns.an[0].type == dpkt.dns.DNS_AAAA: + ip = socket.inet_ntop(socket.AF_INET6, dns.an[0].rdata) + if ip.startswith("fe80"): + interface, _ = sock_info[sock] + ip = f"{ip}%{interface}" + else: + continue + + if (host, ip) not in discovered_list: + print( + f"{Fore.LIGHTBLACK_EX}>> {Fore.LIGHTCYAN_EX}{len(discovered_list)}.{Style.RESET_ALL} {host}: {Fore.LIGHTGREEN_EX}{ip}{Style.RESET_ALL}" + ) + discovered_list.append((host, ip)) + + +def set_remote(address): + ssh_path = os.path.join(os.getenv("HOME"), ".ssh", "config") + try: + # judge input type + if address.find(":") != -1: + ipv6 = True + else: + ipv6 = False + + # find single '%' in host_remote and replace it with double '%%' + address = re.sub(r"(? 2: - print( - "Fatal: Too many arguments provided. Please provide only the IP of the remote host." - ) - sys.exit(1) - -ssh_path = os.path.join(os.getenv("HOME"), ".ssh", "config") -try: - with open(ssh_path, "r") as file: - config = file.read() - updated_config, updated_count = re.subn( - r"Host remote\n HostName [^\n]+", - f"Host remote\n HostName {sys.argv[1]}", - config, - ) - if updated_count == 0: - print("Fatal: Cannot find any place to modify in SSH config.") - sys.exit(1) - with open(ssh_path, "w") as file: - file.write(updated_config) -except FileNotFoundError: - print("Fatal: SSH config file not found.") - sys.exit(1) diff --git a/.script/set-robot b/.script/set-robot new file mode 100755 index 00000000..42197ec9 --- /dev/null +++ b/.script/set-robot @@ -0,0 +1,38 @@ +#!/bin/python3 + +import sys +import re +import os + +if len(sys.argv) <= 1: + print( + "Error: No arguments provided. Please specify the name of the robot type." + ) + sys.exit(1) +elif len(sys.argv) > 2: + print( + "Error: Too many arguments provided. Please specify only the name of the robot type." + ) + sys.exit(1) + +script_paths = [ + os.path.join(os.getenv("HOME"), "env_setup.bash"), + os.path.join(os.getenv("HOME"), "env_setup.zsh"), +] +for script_path in script_paths: + try: + with open(script_path, "r") as file: + config = file.read() + updated_config, updated_count = re.subn( + r"export RMCS_ROBOT_TYPE=\"[^\n\"]*\"", + f'export RMCS_ROBOT_TYPE="{sys.argv[1]}"', + config, + ) + if updated_count == 0: + print(f"Error: Cannot find any place to modify in '{script_path}'.") + sys.exit(1) + with open(script_path, "w") as file: + file.write(updated_config) + except FileNotFoundError: + print(f"Error: '{script_path}' not found.") + sys.exit(1) diff --git a/.script/ssh-remote b/.script/ssh-remote new file mode 100755 index 00000000..bc8b8734 --- /dev/null +++ b/.script/ssh-remote @@ -0,0 +1,7 @@ +#!/bin/bash + +if [ -z "$1" ]; then + ssh -X remote +else + ssh -t -X remote "$@" +fi diff --git a/.script/ssh-remote.sh b/.script/ssh-remote.sh deleted file mode 100755 index 47f12bac..00000000 --- a/.script/ssh-remote.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ssh -X remote diff --git a/.script/sync-remote b/.script/sync-remote new file mode 100755 index 00000000..509d0421 --- /dev/null +++ b/.script/sync-remote @@ -0,0 +1,116 @@ +#!/bin/python3 + +import socket +import selectors +import os +import subprocess + +from colorama import Fore, Style + + +SRC_DIR = os.getenv("RMCS_PATH") + "/rmcs_ws/install" +DST_DIR = "ssh://remote//rmcs_install" +SOCKET_PATH = "/tmp/sync-remote" + +SYNC_FLAG = b"Nothing to do:" +COLORED_SYNC_FLAG = ( + f"{Fore.LIGHTGREEN_EX}{Style.BRIGHT}Nothing to do{Style.RESET_ALL}:".encode("ascii") +) + + +def create_process(): + command = [ + "unison", + "-ignorearchives", + "-auto", + "-batch", + "-repeat", + "watch", + "-times", + SRC_DIR, + DST_DIR, + "-force", + SRC_DIR, + "-follow", + "Name *", + ] + pipe_r, pipe_w = os.pipe() + process = subprocess.Popen( + command, + stdout=pipe_w, + stderr=pipe_w, + ) + return process, pipe_r + + +def create_server(): + if os.path.exists(SOCKET_PATH): + os.remove(SOCKET_PATH) + + server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + server.bind(SOCKET_PATH) + server.listen() + server.setblocking(False) + + return server + + +def finish_connection(conn: socket): + try: + conn.sendall(b"f") + except: + pass + finally: + conn.close() + + +def main(): + selector = selectors.DefaultSelector() + + process, pipe_stdout = create_process() + selector.register(pipe_stdout, selectors.EVENT_READ, data=None) + + server = create_server() + selector.register(server, selectors.EVENT_READ, data=None) + + clients = [] + ready = False + + try: + while True: + events = selector.select(timeout=1) + for key, _ in events: + if key.fileobj == server: + conn, _ = server.accept() + if ready: + finish_connection(conn) + else: + clients.append(conn) + else: + stdout = os.read(pipe_stdout, 1024) + if stdout.startswith(SYNC_FLAG): + ready = True + stdout = COLORED_SYNC_FLAG + stdout[len(SYNC_FLAG) :] + for conn in clients: + finish_connection(conn) + clients = [] + else: + ready = False + os.write(1, stdout) + if process.poll() is not None: + break + except KeyboardInterrupt: + try: + process.wait() + except KeyboardInterrupt: + print("SIGINT sent twice, force quitting.") + process.kill() + finally: + server.close() + selector.close() + if os.path.exists(SOCKET_PATH): + os.unlink(SOCKET_PATH) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/.script/sync-remote.sh b/.script/sync-remote.sh deleted file mode 100755 index 518993c9..00000000 --- a/.script/sync-remote.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash - -SRC_DIR=rmcs_ws/install -DST_DIR=ssh://remote//rmcs_install - -unison -ignorearchives -auto -batch -repeat watch -times ${SRC_DIR} ${DST_DIR} -force ${SRC_DIR} \ No newline at end of file diff --git a/.script/template/entrypoint b/.script/template/entrypoint new file mode 100644 index 00000000..e8660e7a --- /dev/null +++ b/.script/template/entrypoint @@ -0,0 +1,17 @@ +#!/usr/bin/bash + +# Remove all files in /tmp +rm -rf /tmp/* + +# Remove all files in /run except for /run/dbus +find /run -mindepth 1 -not -path "/run/dbus*" -exec rm -rf {} + + +service rmcs start + +service ssh start + +if [ -f "/etc/avahi/enabled" ]; then + service avahi-daemon start +fi + +sleep infinity \ No newline at end of file diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash new file mode 100644 index 00000000..2af7ba35 --- /dev/null +++ b/.script/template/env_setup.bash @@ -0,0 +1,15 @@ +#!/bin/bash + +export ROS_LOCALHOST_ONLY=1 +export RCUTILS_COLORIZED_OUTPUT=1 +export RMCS_PATH="/workspaces/RMCS" + +source /opt/ros/jazzy/setup.bash + +if [ -f "/rmcs_install/local_setup.bash" ]; then + source /rmcs_install/local_setup.bash +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.bash" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.bash +fi + +export RMCS_ROBOT_TYPE="" \ No newline at end of file diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh new file mode 100644 index 00000000..b70409b5 --- /dev/null +++ b/.script/template/env_setup.zsh @@ -0,0 +1,22 @@ +#!/bin/bash + +export RCUTILS_COLORIZED_OUTPUT=1 +export ROS_LOCALHOST_ONLY=1 +export RMCS_PATH="/workspaces/RMCS" + +source /opt/ros/jazzy/setup.zsh + +if [ -f "/rmcs_install/local_setup.zsh" ]; then + source /rmcs_install/local_setup.zsh +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.zsh" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.zsh +fi + +eval "$(register-python-argcomplete ros2)" +eval "$(register-python-argcomplete colcon)" + +export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit \ No newline at end of file diff --git a/.script/template/rmcs-service b/.script/template/rmcs-service new file mode 100644 index 00000000..177cf6ae --- /dev/null +++ b/.script/template/rmcs-service @@ -0,0 +1,73 @@ +#! /bin/bash + +set -e + +start() { + if [[ -z "$HOME" ]]; then + export HOME=/root + fi + + if is_running; then + echo 'Error: RMCS daemon is still running!' + exit 1 + fi + + source ~/env_setup.bash + + if [ -z "${RMCS_ROBOT_TYPE}" ]; then + echo "Error: Please set robot type first (e.g. 'set-robot sentry')." + exit 1 + fi + + screen -dmS rmcs bash -c "ros2 launch rmcs_bringup rmcs.launch.py robot:=${RMCS_ROBOT_TYPE}" + + sleep 0.1 + + if is_running; then + echo "Successfully started RMCS daemon." + else + echo "Error: Failed to start RMCS daemon." + exit 1 + fi +} + +attach() { + screen -r rmcs +} + +stop() { + if is_running; then + screen -S rmcs -X quit + echo "Successfully stopped RMCS daemon." + fi +} + +is_running() { + if ls '/run/screen/S-root' 2>/dev/null | grep -q '.*\.rmcs'; then + return 0 + else + return 1 + fi +} + +case "$1" in + start) + start + ;; + attach) + attach + ;; + stop) + stop + ;; + restart) + stop + start + ;; + *) + echo 'Usage: service rmcs {start|attach|stop|restart}' + exit 1 + ;; +esac + +exit 0 \ No newline at end of file diff --git a/.script/template/set-hostname b/.script/template/set-hostname new file mode 100644 index 00000000..79055abf --- /dev/null +++ b/.script/template/set-hostname @@ -0,0 +1,27 @@ +#!/bin/bash + +# Check if exactly one argument is passed +if [ "$#" -ne 1 ]; then + echo "Usage: set-hostname " + echo "Example: set-hostname my-sentry" + exit 1 +fi + +# Handle the "off" argument +if [ "$1" == "off" ]; then + rm -f /etc/avahi/enabled + service avahi-daemon stop + exit 0 +fi + +# Update the avahi-daemon.conf file +AVAHI_CONF="/etc/avahi/avahi-daemon.conf" +if [ -f "$AVAHI_CONF" ]; then + touch /etc/avahi/enabled + sed -i -E 's|^#?host-name=\S*|host-name='"$1"'|' "$AVAHI_CONF" + service avahi-daemon restart + echo "Updated host-name in $AVAHI_CONF to $1." +else + echo "Error: Configuration file $AVAHI_CONF does not exist." + exit 1 +fi \ No newline at end of file diff --git a/.script/wait-sync b/.script/wait-sync new file mode 100755 index 00000000..96eb0650 --- /dev/null +++ b/.script/wait-sync @@ -0,0 +1,30 @@ +#!/bin/python3 + +import socket +import os + +SOCKET_PATH = "/tmp/sync-remote" + +def wait_for_sync_complete(): + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as client_socket: + try: + client_socket.connect(SOCKET_PATH) + data = client_socket.recv(1) + if data == b"f": + return 0 + elif data == b"": + print(f"Error: Synchronization stopped.") + return 1 + else: + print(f"Error: Received unexpected data!") + return 2 + except FileNotFoundError: + print("Sync not in progress! Start synchronization with the command 'sync-remote'.") + return 10 + except Exception as e: + print(f"Error: {e}") + return 11 + + +if __name__ == "__main__": + exit(wait_for_sync_complete()) diff --git a/.vscode/settings.json b/.vscode/settings.json index 36ad8788..a6bd64fd 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -11,5 +11,9 @@ "colcon", "rmcs", "rosdep" - ] + ], + "clangd.arguments": [ + "--query-driver=/usr/bin/c*,/usr/bin/g*,/usr/bin/C*" + ], + "clangd.path": "/usr/bin/clangd" } \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 9ca17a98..0bee27a2 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,46 +4,68 @@ # Base container, provides a runtime environment -FROM ros:humble AS rmcs-base +FROM ros:jazzy AS rmcs-base # Change bash as default shell instead of sh SHELL ["/bin/bash", "-c"] -# Install some tools and libraries. -RUN apt-get update && apt-get -y install \ +# Set timezone and non-interactive mode +ENV TZ=Asia/Shanghai \ + DEBIAN_FRONTEND=noninteractive + +# Install tools and libraries. +RUN apt-get update && apt-get install -y --no-install-recommends \ vim wget curl unzip \ - build-essential \ - cmake \ - make ninja-build \ + zsh screen tmux \ + usbutils net-tools iputils-ping \ + gstreamer1.0-tools gstreamer1.0-plugins-base gstreamer1.0-plugins-good \ + ripgrep htop fzf \ + libusb-1.0-0-dev \ libeigen3-dev \ libopencv-dev \ - libgoogle-glog-dev libgflags-dev \ - libatlas-base-dev libsuitesparse-dev \ + libgoogle-glog-dev \ + libgflags-dev \ + libatlas-base-dev \ + libsuitesparse-dev \ libceres-dev \ - && rm -rf /var/lib/apt/lists/* + ros-$ROS_DISTRO-rviz2 \ + ros-$ROS_DISTRO-foxglove-bridge \ + dotnet-sdk-8.0 \ + ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-msgs && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* + -# Install openvino 2023.3 runtime (C++ API only) +# Install openvino runtime RUN wget https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - apt-key add GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ - echo "deb https://apt.repos.intel.com/openvino/2023 ubuntu22 main" | tee /etc/apt/sources.list.d/intel-openvino-2023.list && \ + apt-key add ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ + rm ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB && \ + echo "deb https://apt.repos.intel.com/openvino ubuntu24 main" > /etc/apt/sources.list.d/intel-openvino.list && \ apt-get update && \ - apt-get install -y openvino-2023.3.0 && \ - rm -rf /var/lib/apt/lists/* && \ - rm ./GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB - -# Mount rmcs source, fix dependencies and compile -RUN --mount=type=bind,target=/tmp/rmcs_ws/src,source=rmcs_ws/src,readonly \ - cd /tmp/rmcs_ws && \ - source /opt/ros/humble/local_setup.bash && \ + apt-get install -y --no-install-recommends openvino-2025.2.0 && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* + +# Install Livox SDK +RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git && \ + cd Livox-SDK2 && \ + sed -i '6iset(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-pragmas -Wno-c++20-compat -include cstdint")' CMakeLists.txt && \ + mkdir build && cd build && \ + cmake -DCMAKE_BUILD_TYPE=Release .. && \ + make -j && \ + make install && \ + cd ../.. && rm -rf Livox-SDK2 + +# Mount rmcs source and install dependencies +RUN --mount=type=bind,target=/rmcs_ws/src,source=rmcs_ws/src,readonly \ apt-get update && \ - rosdep install --from-paths src --ignore-src -r -y && \ - rm -rf /var/lib/apt/lists/* && \ - colcon build --install-base /rmcs_install --merge-install && \ - rm -rf build log + rosdep install --from-paths /rmcs_ws/src --ignore-src -r -y && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* # Install unison to allow file synchronization RUN cd /tmp && \ - wget -O unison.tar.gz https://github.com/bcpierce00/unison/releases/download/v2.53.4/unison-2.53.4-ubuntu-x86_64.tar.gz && \ + wget -O unison.tar.gz https://github.com/bcpierce00/unison/releases/download/v2.53.7/unison-2.53.7-ubuntu-x86_64.tar.gz && \ mkdir -p unison && tar -zxf unison.tar.gz -C unison && \ cp unison/bin/* /usr/local/bin && \ rm -rf unison unison.tar.gz @@ -53,73 +75,95 @@ RUN cd /tmp && \ # Developing container, works with devcontainer FROM rmcs-base AS rmcs-develop -# Install develop tools (ssh/clangd/zsh/etc...) -RUN apt-get update && apt-get -y install \ +# Install develop tools +RUN apt-get update && apt-get install -y --no-install-recommends \ + libc6-dev gcc-14 g++-14 \ + cmake make ninja-build \ openssh-client \ - lsb-release software-properties-common gnupg zsh sudo \ - libcanberra-gtk-module libcanberra-gtk3-module && \ - rm -rf /var/lib/apt/lists/* \ - echo -e "\n" | bash -c "$(wget -O - https://apt.llvm.org/llvm.sh)" && \ - ln -s /usr/bin/clangd-* /usr/bin/clangd - -# Add user -RUN useradd -m developer --shell /bin/zsh && echo "developer:developer" | chpasswd && adduser developer sudo && \ - echo "developer ALL=(ALL:ALL) NOPASSWD:ALL" >> /etc/sudoers && \ - gpasswd --add developer dialout -WORKDIR /home/developer -ENV USER=developer -ENV WORKDIR=/home/developer - -# Generate ssh secret key & profile unison -RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh,readonly \ - mkdir -p .ssh && \ - if [ ! -f "/tmp/.ssh/id_rsa.pub" ]; then ssh-keygen -N "" -f ".ssh/id_rsa"; fi && \ + lsb-release software-properties-common gnupg sudo \ + python3-colorama python3-dpkt && \ + wget -O ./llvm-snapshot.gpg.key https://apt.llvm.org/llvm-snapshot.gpg.key && \ + apt-key add ./llvm-snapshot.gpg.key && \ + rm ./llvm-snapshot.gpg.key && \ + echo "deb https://apt.llvm.org/noble/ llvm-toolchain-noble main" > /etc/apt/sources.list.d/llvm-apt.list && \ + apt-get update && \ + version=`apt-cache search clangd- | grep clangd- | awk -F' ' '{print $1}' | sort -V | tail -1 | cut -d- -f2` && \ + apt-get install -y --no-install-recommends clangd-$version && \ + update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-14 50 && \ + update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-14 50 && \ + update-alternatives --install /usr/bin/clangd clangd /usr/bin/clangd-$version 50 && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* + +# Generate/load ssh key and setup unison +RUN --mount=type=bind,target=/tmp/.ssh,source=.ssh,readonly=false \ + cd /home/ubuntu && mkdir -p .ssh && \ + if [ ! -f "/tmp/.ssh/id_rsa" ]; then ssh-keygen -N "" -f "/tmp/.ssh/id_rsa"; fi && \ cp -r /tmp/.ssh/* .ssh && \ + chown -R 1000:1000 .ssh && chmod 600 .ssh/id_rsa && \ mkdir -p .unison && \ echo 'confirmbigdel = false' >> ".unison/default.prf" && \ - chown -R 1000:1000 .ssh .unison - -USER developer - -# Install oh my zsh & change theme to af-magic -RUN set -eo pipefail && \ - curl -fsSL https://raw.github.com/robbyrussell/oh-my-zsh/master/tools/install.sh | sh && \ - sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' .zshrc - -COPY --chown=root:root .script/set-remote.py /usr/local/bin/set-remote -COPY --chown=root:root .script/ssh-remote.sh /usr/local/bin/ssh-remote -COPY --chown=root:root .script/sync-remote.sh /usr/local/bin/sync-remote -COPY --chown=root:root .script/service-remote.sh /usr/local/bin/service-remote - - + chown -R 1000:1000 .unison + +# Install latest neovim +RUN curl -LO https://github.com/neovim/neovim/releases/latest/download/nvim-linux-x86_64.tar.gz && \ + rm -rf /opt/nvim && \ + tar -C /opt -xzf nvim-linux-x86_64.tar.gz && \ + rm nvim-linux-x86_64.tar.gz + +# Change user +RUN chsh -s /bin/zsh ubuntu && \ + echo "ubuntu ALL=(ALL:ALL) NOPASSWD:ALL" >> /etc/sudoers +WORKDIR /home/ubuntu +ENV USER=ubuntu +ENV WORKDIR=/home/ubuntu +USER ubuntu + +# Install oh my zsh, change theme to af-magic and setup environment of zsh +RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)" && \ + sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' ~/.zshrc && \ + echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ + echo 'export PATH="${PATH}:/opt/nvim-linux-x86_64/bin"' >> ~/.zshrc && \ + echo 'export PATH="${PATH}:${RMCS_PATH}/.script"' >> ~/.zshrc + +# Copy environment setup scripts +COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash +COPY --chown=1000:1000 .script/template/env_setup.zsh env_setup.zsh # Runtime container, will automatically launch the main program FROM rmcs-base AS rmcs-runtime -# Install ssh server +# Install runtime tools RUN apt-get update && \ - apt-get install -y openssh-server && \ - rm -rf /var/lib/apt/lists/* && \ + apt-get install -y --no-install-recommends tini openssh-server avahi-daemon orphan-sysvinit-scripts && \ + apt-get autoremove -y && apt-get clean && \ + rm -rf /var/lib/apt/lists/* /tmp/* && \ echo 'Port 2022' >> /etc/ssh/sshd_config && \ echo 'PermitRootLogin yes' >> /etc/ssh/sshd_config && \ - echo 'PasswordAuthentication no' >> /etc/ssh/sshd_config + echo 'PasswordAuthentication no' >> /etc/ssh/sshd_config && \ + sed -i 's/#enable-dbus=yes/enable-dbus=no/g' /etc/avahi/avahi-daemon.conf + +# Install oh my zsh, disable plugins and update, setup environment and set zsh as default shell +RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools/install.sh -O -)" && \ + sed -i 's/plugins=(git)/plugins=()/g' ~/.zshrc && \ + sed -i "s/# zstyle ':omz:update' mode disabled/zstyle ':omz:update' mode disabled/g" ~/.zshrc && \ + echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ + echo 'export PATH=${PATH}:/rmcs_install/lib/rmcs_cli' >> ~/.zshrc && \ + chsh -s /bin/zsh root + +RUN mkdir -p /rmcs_install/ -# Add tini -RUN wget -O /tini https://github.com/krallin/tini/releases/download/v0.19.0/tini && \ - chmod +x /tini +COPY --chown=root:root .script/set-robot /usr/local/bin/set-robot +COPY --chown=root:root .script/template/set-hostname /usr/local/bin/set-hostname -# Entrypoint -RUN echo '#!/usr/bin/bash' > /entrypoint.sh && \ - echo 'rm -rf /tmp/*' >> /entrypoint.sh && \ - echo 'service ssh restart' >> /entrypoint.sh && \ - echo 'service rmcs restart' >> /entrypoint.sh && \ - echo 'sleep infinity' >> /entrypoint.sh && \ - chmod +x /entrypoint.sh +COPY --chown=root:root .script/template/entrypoint /entrypoint +COPY --chown=root:root .script/template/rmcs-service /etc/init.d/rmcs -COPY --chown=root:root .script/service.sh /etc/init.d/rmcs -COPY --from=rmcs-develop --chmod=600 --chown=root:root /home/developer/.ssh/id_rsa.pub /root/.ssh/authorized_keys +COPY --from=rmcs-develop --chown=root:root /home/ubuntu/.ssh/id_rsa.pub /root/.ssh/authorized_keys WORKDIR /root/ +COPY --chown=root:root .script/template/env_setup.bash env_setup.bash +COPY --chown=root:root .script/template/env_setup.zsh env_setup.zsh -ENTRYPOINT ["/tini", "--"] -CMD [ "/entrypoint.sh" ] \ No newline at end of file +ENTRYPOINT ["tini", "--"] +CMD [ "/entrypoint" ] \ No newline at end of file diff --git a/rmcs_ws/src/fast_tf b/rmcs_ws/src/fast_tf index d3a39dfb..713e2a40 160000 --- a/rmcs_ws/src/fast_tf +++ b/rmcs_ws/src/fast_tf @@ -1 +1 @@ -Subproject commit d3a39dfb1d340516ffc3e69703c72201736dab78 +Subproject commit 713e2a40f18acae36dfd2f87e67c1c813405b37a diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera index c0adb451..815952fd 160000 --- a/rmcs_ws/src/hikcamera +++ b/rmcs_ws/src/hikcamera @@ -1 +1 @@ -Subproject commit c0adb451ed9698a351d574d0891641d697404437 +Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 diff --git a/rmcs_ws/src/rmcs_auto_aim b/rmcs_ws/src/rmcs_auto_aim deleted file mode 160000 index 641023eb..00000000 --- a/rmcs_ws/src/rmcs_auto_aim +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 641023eb330436bf2f399b293e8d1756138d662f diff --git a/rmcs_ws/src/rmcs_bringup/config/engineer.yaml b/rmcs_ws/src/rmcs_bringup/config/engineer.yaml new file mode 100644 index 00000000..c9d6bd65 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/engineer.yaml @@ -0,0 +1,330 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Engineer -> engineer_hardware + - rmcs_core::controller::arm::ArmController -> arm_controller + + - rmcs_core::controller::leg::LegController -> leg_controller + - rmcs_core::controller::chassis::Chassis_Controller -> chassis_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint6_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint6_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint5_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint5_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint4_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint4_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint3_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint3_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint2_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint2_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint1_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint1_velocity_pid_controller + + - rmcs_core::controller::pid::PidController -> leg_omni_l_velocity_controller + - rmcs_core::controller::pid::PidController -> leg_omni_r_velocity_controller + + - rmcs_core::controller::pid::ErrorPidController -> big_yaw_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> big_yaw_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> leg_lf_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> leg_lf_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> leg_rf_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> leg_rf_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> leg_lb_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> leg_lb_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> leg_rb_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> leg_rb_velocity_pid_controller + + - rmcs_core::controller::pid::PidController -> arm_pump_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> mine_pump_velocity_pid_controller + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster +engineer_hardware: + ros__parameters: + arm_board_usb_pid: 0x1783 + left_board_usb_pid: 0x652f + right_board_usb_pid: 0x75f3 + joint6_zero_point: 0 + joint5_zero_point: 37361 + joint4_zero_point: 12200 + joint3_zero_point: 260992 + joint2_zero_point: 52343 + joint6_qlim: [-3.141592,3.141592] + joint5_qlim: [-1.93532,1.93532] + joint4_qlim: [-3.141592,3.141592] + joint3_qlim: [-1.0472,0.8727] + joint2_qlim: [-1.308,1.16719] + steering_lf_zero_point: 3399 + steering_lb_zero_point: 681 + steering_rb_zero_point: 693 + steering_rf_zero_point: 5435 + leg_lf_ecd_zero_point: 109021 + leg_lb_ecd_zero_point: 153207 + leg_rb_ecd_zero_point: 2135 + leg_rf_ecd_zero_point: 133400 + big_yaw_zero_point: 20485 + + +leg_controller: + ros__parameters: + forward_x_position_in_FourWheel: 243.0 + forward_x_position_in_SixWheel: 250.0 + backward_x_position_in_SixWheel: 221.0 + initial_end_point: [1.700270, 1.680, 1.680, 1.700270] + press_end_point: [0.799803, 0.778351, 0.778351, 0.799803] + lift_end_point: [0.902222, 1.413787, 1.413787,0.902222] + lift_and_initial_end_point: [1.400270, 1.410, 1.410, 1.400270] + initial_again_end_point: [1.500270, 1.480, 1.480, 1.500270] + k : [ -600.00, -600.0, -300.0,-300.0] + b : [400.0, 500.0, 500.0, 900.0] # press lift initial_again lift_and_initial + + +joint6_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint6/control_angle_error + control: /arm/Joint6/control_velocity + kp: 9620.0 + ki: 0.0 + kd: 25.0 + +joint6_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint6/velocity + setpoint: /arm/Joint6/control_velocity + control: /arm/Joint6/control_torque + kp: 0.021 + ki: 0.0 + kd: 0.0005 + +joint5_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint5/control_angle_error + control: /arm/Joint5/control_velocity + kp: 6920.0 + ki: 0.0 + kd: 15.0 + +joint5_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint5/velocity + setpoint: /arm/Joint5/control_velocity + control: /arm/Joint5/control_torque + kp: 0.028 + ki: 0.0 + kd: 0.0009 + +joint4_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint4/control_angle_error + control: /arm/Joint4/control_velocity + kp: 3120.0 + ki: 0.0 + kd: 15.0 +joint4_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint4/velocity + setpoint: /arm/Joint4/control_velocity + control: /arm/Joint4/control_torque + kp: 0.06 + ki: 0.0 + kd: 0.001 + +joint3_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint3/control_angle_error + control: /arm/Joint3/control_velocity + kp: 4290.0 + ki: 0.0 + kd: 20.0 +joint3_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint3/velocity + setpoint: /arm/Joint3/control_velocity + control: /arm/Joint3/control_torque + kp: 0.0025 + ki: 0.0 + kd: 0.0002 + +joint2_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint2/control_angle_error + control: /arm/Joint2/control_velocity + kp: 4390.0 + ki: 0.0 + kd: 20.0 +joint2_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint2/velocity + setpoint: /arm/Joint2/control_velocity + control: /arm/Joint2/control_torque + kp: 0.00293 + ki: 0.0 + kd: 0.00004 + +joint1_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint1/control_angle_error + control: /arm/Joint1/control_velocity + kp: 6520.0 + ki: 0.0 + kd: 40.0 +joint1_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint1/velocity + setpoint: /arm/Joint1/control_velocity + control: /arm/Joint1/control_torque + kp: 0.042 + ki: 0.0 + kd: 0.0001 + +leg_omni_l_velocity_controller: + ros__parameters: + measurement: /leg/omni/l/velocity + setpoint: /leg/omni/l/target_vel + control: /leg/omni/l/control_torque + kp: 0.7 + ki: 0.0 + kd: 0.0 + +leg_omni_r_velocity_controller: + ros__parameters: + measurement: /leg/omni/r/velocity + setpoint: /leg/omni/r/target_vel + control: /leg/omni/r/control_torque + kp: 0.7 + ki: 0.0 + kd: 0.0 + +big_yaw_angle_error_pid_controller: + ros__parameters: + measurement: /chassis/big_yaw/target_angle_error + control: /chassis/big_yaw/control_vel + kp: 91.3 + ki: 0.0 + kd: 2.6 +big_yaw_velocity_pid_controller: + ros__parameters: + measurement: /chassis/big_yaw/velocity + setpoint: /chassis/big_yaw/control_vel + control: /chassis/big_yaw/control_torque + kp: 3.99 + ki: 0.0 + kd: 0.039 + + +leg_lf_angle_error_pid_controller: + ros__parameters: + measurement: /leg/joint/lf/control_theta_error + control: /leg/joint/lf/control_vel + kp: 150.0 + ki: 0.0 + kd: 5.0 +leg_lf_velocity_pid_controller: + ros__parameters: + measurement: /leg/joint/lf/velocity + setpoint: /leg/joint/lf/control_vel + control: /leg/joint/lf/control_torque + kp: 2.0 + ki: 0.0 + kd: 0.2 + +leg_rf_angle_error_pid_controller: + ros__parameters: + measurement: /leg/joint/rf/control_theta_error + control: /leg/joint/rf/control_vel + kp: 150.0 + ki: 0.0 + kd: 5.0 + + +leg_rf_velocity_pid_controller: + ros__parameters: + measurement: /leg/joint/rf/velocity + setpoint: /leg/joint/rf/control_vel + control: /leg/joint/rf/control_torque + kp: 2.0 + ki: 0.0 + kd: 0.2 + +leg_lb_angle_error_pid_controller: + ros__parameters: + measurement: /leg/joint/lb/control_theta_error + control: /leg/joint/lb/control_vel + kp: 6000.0 + ki: 0.0 + kd: 0.0 +leg_lb_velocity_pid_controller: + ros__parameters: + measurement: /leg/joint/lb/velocity + setpoint: /leg/joint/lb/control_vel + control: /leg/joint/lb/control_torque + kp: 0.03 + ki: 0.0 + kd: 0.0 + +leg_rb_angle_error_pid_controller: + ros__parameters: + measurement: /leg/joint/rb/control_theta_error + control: /leg/joint/rb/control_vel + kp: 6000.0 + ki: 0.0 + kd: 0.0 + +leg_rb_velocity_pid_controller: + ros__parameters: + measurement: /leg/joint/rb/velocity + setpoint: /leg/joint/rb/control_vel + control: /leg/joint/rb/control_torque + kp: 0.03 + ki: 0.0 + kd: 0.0 + +steering_wheel_controller: + ros__parameters: + mess: 30.0 + moment_of_inertia: 1.0 + vehicle_radius: 0.3238 + wheel_radius: 0.055 + friction_coefficient: 0.6 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 15.37 + +arm_pump_velocity_pid_controller: + ros__parameters: + measurement: /arm/pump/velocity + setpoint: /arm/pump/target_vel + control: /arm/pump/control_torque + kp: 0.001 + ki: 0.0 + kd: 0.0 +mine_pump_velocity_pid_controller: + ros__parameters: + measurement: /mine/pump/velocity + setpoint: /mine/pump/target_vel + control: /mine/pump/control_torque + kp: 0.001 + ki: 0.0 + kd: 0.0 + + +value_broadcaster: + ros__parameters: + forward_list: + - "/leg/lf_result" + - "/leg/lb_result" + - "/leg/rf_result" + - "/leg/rb_result" + - "/leg/encoder/lf/angle" + - "/leg/encoder/lb/angle" + - "/leg/encoder/rb/angle" + - "/leg/encoder/rf/angle" + - "/leg/velocity_in_x" + diff --git a/rmcs_ws/src/rmcs_bringup/config/infantry_a.yaml b/rmcs_ws/src/rmcs_bringup/config/infantry_a.yaml index 96115ecd..56c602ce 100644 --- a/rmcs_ws/src/rmcs_bringup/config/infantry_a.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/infantry_a.yaml @@ -37,7 +37,7 @@ rmcs_executor: infantry_hardware: ros__parameters: - usb_pid: 0x488D + usb_pid: 0x03d9 yaw_motor_zero_point: 7830 pitch_motor_zero_point: 2028 imu_gx_bias: +0.004977 diff --git a/rmcs_ws/src/rmcs_bringup/config/infantry_b.yaml b/rmcs_ws/src/rmcs_bringup/config/infantry_b.yaml index b573789c..0a263861 100644 --- a/rmcs_ws/src/rmcs_bringup/config/infantry_b.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/infantry_b.yaml @@ -37,7 +37,7 @@ rmcs_executor: infantry_hardware: ros__parameters: - usb_pid: 0xac89 + usb_pid: 0x03d9 yaw_motor_zero_point: 4425 pitch_motor_zero_point: 6716 imu_gx_bias: +0.001294 diff --git a/rmcs_ws/src/rmcs_bringup/config/lk_robot.yaml b/rmcs_ws/src/rmcs_bringup/config/lk_robot.yaml new file mode 100644 index 00000000..ba8bcda9 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/lk_robot.yaml @@ -0,0 +1,249 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::LK_Robot -> hardware + - rmcs_core::controller::arm::Arm -> arm_controller + - rmcs_core::controller::chassis::Steering -> steering_chassis + - rmcs_core::controller::pid::ErrorPidController -> joint6_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint6_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint5_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint5_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint4_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint4_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint3_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint3_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint2_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint2_velocity_pid_controller + + - rmcs_core::controller::pid::ErrorPidController -> joint1_angle_pid_controller + - rmcs_core::controller::pid::PidController -> joint1_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> steering_steering_lf_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> steering_steering_lf_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> steering_steering_lb_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> steering_steering_lb_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> steering_steering_rb_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> steering_steering_rb_velocity_pid_controller + - rmcs_core::controller::pid::ErrorPidController -> steering_steering_rf_angle_error_pid_controller + - rmcs_core::controller::pid::PidController -> steering_steering_rf_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> steering_wheel_rb_velocity_controller + - rmcs_core::controller::pid::PidController -> steering_wheel_rf_velocity_controller + - rmcs_core::controller::pid::PidController -> steering_wheel_lf_velocity_controller + - rmcs_core::controller::pid::PidController -> steering_wheel_lb_velocity_controller + +hardware: + ros__parameters: + right_board_usb_pid: 0x94dc + left_board_usb_pid: 0x93ac + right_forward_zero_point: 30130 + right_back_zero_point: 49504 + left_forward_zero_point: 11648 + left_back_zero_point: 4477 + joint6_zero_point: 0 + joint5_zero_point: 40661 + joint4_zero_point: 12200 + joint3_zero_point: 2000 + joint2_zero_point: 196350 + joint1_zero_point: 28349 + joint6_qlim: [-3.141592,3.141592] + joint5_qlim: [-1.93532,1.93532] + joint4_qlim: [-3.141592,3.141592] + joint3_qlim: [-1.01,0.9227] + joint2_qlim: [-1.0108,1.09719] + joint1_qlim: [-2.841592,2.841592] +joint6_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint6/control_angle_error + control: /arm/Joint6/control_velocity + kp: 9620.0 + ki: 0.0 + kd: 25.0 + +joint6_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint6/velocity + setpoint: /arm/Joint6/control_velocity + control: /arm/Joint6/control_torque + kp: 0.021 + ki: 0.0 + kd: 0.0005 + +joint5_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint5/control_angle_error + control: /arm/Joint5/control_velocity + kp: 6920.0 + ki: 0.0 + kd: 15.0 +joint5_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint5/velocity + setpoint: /arm/Joint5/control_velocity + control: /arm/Joint5/control_torque + kp: 0.028 + ki: 0.0 + kd: 0.0009 + +joint4_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint4/control_angle_error + control: /arm/Joint4/control_velocity + kp: 3120.0 + ki: 0.0 + kd: 15.0 +joint4_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint4/velocity + setpoint: /arm/Joint4/control_velocity + control: /arm/Joint4/control_torque + kp: 0.06 + ki: 0.0 + kd: 0.001 + +joint3_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint3/control_angle_error + control: /arm/Joint3/control_velocity + kp: 4290.0 + ki: 0.0 + kd: 20.0 +joint3_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint3/velocity + setpoint: /arm/Joint3/control_velocity + control: /arm/Joint3/control_torque + kp: 0.0025 + ki: 0.0 + kd: 0.0002 + +joint2_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint2/control_angle_error + control: /arm/Joint2/control_velocity + kp: 6390.0 + ki: 0.0 + kd: 20.0 +joint2_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint2/velocity + setpoint: /arm/Joint2/control_velocity + control: /arm/Joint2/control_torque + kp: 0.00293 + ki: 0.0 + kd: 0.00004 + +joint1_angle_pid_controller: + ros__parameters: + measurement: /arm/Joint1/control_angle_error + control: /arm/Joint1/control_velocity + kp: 9520.0 + ki: 0.0 + kd: 40.0 +joint1_velocity_pid_controller: + ros__parameters: + measurement: /arm/Joint1/velocity + setpoint: /arm/Joint1/control_velocity + control: /arm/Joint1/control_torque + kp: 0.032 + ki: 0.0 + kd: 0.0001 + +steering_steering_rb_angle_error_pid_controller: + ros__parameters: + measurement: /steering/steering/rb/target_angle_error + control: /steering/steering/rb/control_vel + kp: 60.0 + ki: 0.0 + kd: 0.0 +steering_steering_rb_velocity_pid_controller: + ros__parameters: + measurement: /steering/steering/rb/velocity + setpoint: /steering/steering/rb/control_vel + control: /steering/steering/rb/control_torque + kp: 0.08 + ki: 0.0 + kd: 0.0 +steering_wheel_rb_velocity_controller: + ros__parameters: + measurement: /steering/wheel/rb/velocity + setpoint: /steering/wheel/rb/target_vel + control: /steering/wheel/rb/control_torque + kp: 0.4 + ki: 0.0 + kd: 0.0 + +steering_steering_rf_angle_error_pid_controller: + ros__parameters: + measurement: /steering/steering/rf/target_angle_error + control: /steering/steering/rf/control_vel + kp: 60.0 + ki: 0.0 + kd: 0.0 +steering_steering_rf_velocity_pid_controller: + ros__parameters: + measurement: /steering/steering/rf/velocity + setpoint: /steering/steering/rf/control_vel + control: /steering/steering/rf/control_torque + kp: 0.08 + ki: 0.0 + kd: 0.0 +steering_wheel_rf_velocity_controller: + ros__parameters: + measurement: /steering/wheel/rf/velocity + setpoint: /steering/wheel/rf/target_vel + control: /steering/wheel/rf/control_torque + kp: 0.4 + ki: 0.0 + kd: 0.0 + +steering_steering_lf_angle_error_pid_controller: + ros__parameters: + measurement: /steering/steering/lf/target_angle_error + control: /steering/steering/lf/control_vel + kp: 60.0 + ki: 0.0 + kd: 0.0 +steering_steering_lf_velocity_pid_controller: + ros__parameters: + measurement: /steering/steering/lf/velocity + setpoint: /steering/steering/lf/control_vel + control: /steering/steering/lf/control_torque + kp: 0.08 + ki: 0.0 + kd: 0.0 +steering_wheel_lf_velocity_controller: + ros__parameters: + measurement: /steering/wheel/lf/velocity + setpoint: /steering/wheel/lf/target_vel + control: /steering/wheel/lf/control_torque + kp: 0.4 + ki: 0.0 + kd: 0.0 + +steering_steering_lb_angle_error_pid_controller: + ros__parameters: + measurement: /steering/steering/lb/target_angle_error + control: /steering/steering/lb/control_vel + kp: 60.0 + ki: 0.0 + kd: 0.0 +steering_steering_lb_velocity_pid_controller: + ros__parameters: + measurement: /steering/steering/lb/velocity + setpoint: /steering/steering/lb/control_vel + control: /steering/steering/lb/control_torque + kp: 0.08 + ki: 0.0 + kd: 0.01 +steering_wheel_lb_velocity_controller: + ros__parameters: + measurement: /steering/wheel/lb/velocity + setpoint: /steering/wheel/lb/target_vel + control: /steering/wheel/lb/control_torque + kp: 0.4 + ki: 0.0 + kd: 0.0 diff --git a/rmcs_ws/src/rmcs_bringup/config/test.yaml b/rmcs_ws/src/rmcs_bringup/config/test.yaml new file mode 100644 index 00000000..fc80192e --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/test.yaml @@ -0,0 +1,10 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - "rmcs_core::hardware::Test -> test_hardware" + - "rmcs_core::broadcaster::ValueBroadcaster -> Value_Broadcaster" + +test_hardware: + ros__parameters: + usb_pid_bottom_board: 0x75f3 diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index c9e11f34..ed0893ef 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -1,28 +1,62 @@ -from launch import LaunchDescription +from typing import List, Optional +import os + +from launch import ( + LaunchContext, + LaunchDescription, + LaunchDescriptionEntity, +) +from launch.actions import LogInfo +from launch.substitutions import LaunchConfiguration + from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -def generate_launch_description(): - ld = LaunchDescription() - - ld.add_action( - Node( - package="rmcs_executor", - executable="rmcs_executor", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare("rmcs_bringup"), +class MyLaunchDescriptionEntity(LaunchDescriptionEntity): + def visit( + self, context: "LaunchContext" + ) -> Optional[List["LaunchDescriptionEntity"]]: + entities = [] + + robot_config = LaunchConfiguration("robot").perform(context) + if robot_config.startswith("auto."): + is_automatic = True + robot_name = robot_config[5:] + else: + is_automatic = False + robot_name = robot_config + + entities.append( + LogInfo( + msg=f"Starting RMCS on robot '{robot_config}'{'(automatic)' if is_automatic else ''} -> {robot_name}.yaml" + ) + ) + + entities.append( + Node( + package="rmcs_executor", + executable="rmcs_executor", + parameters=[ + os.path.join( + FindPackageShare("rmcs_bringup").perform(context), "config", - LaunchConfiguration("config"), - ] - ) - ], - respawn=True, - respawn_delay=5.0, + robot_name + ".yaml", + ), + ], + respawn=True, + respawn_delay=1.0, + output="screen", + ) ) - ) - return ld + if is_automatic: + pass + + return entities + + +def generate_launch_description(): + ld = LaunchDescription([MyLaunchDescriptionEntity()]) + + return ld \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index cbd8f6e3..43ad6568 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -4,12 +4,15 @@ project(rmcs_core) set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() find_package (ament_cmake_auto REQUIRED) + + ament_auto_find_build_dependencies () file (GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS @@ -24,8 +27,12 @@ ament_auto_add_library ( include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) +include_directories(${PROJECT_SOURCE_DIR}/librmcs) + +include_directories(SYSTEM "/usr/include/libusb-1.0") + +target_link_libraries(${PROJECT_NAME} -lusb-1.0 atomic) -target_link_libraries(${PROJECT_NAME} -lusb-1.0) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs new file mode 160000 index 00000000..b82f2eaf --- /dev/null +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -0,0 +1 @@ +Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 7ec589fc..c1807312 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -10,6 +10,8 @@ ament_cmake rclcpp + + ros2bag std_msgs pluginlib tf2 @@ -19,6 +21,7 @@ rmcs_msgs rmcs_executor rmcs_description + atomic ament_lint_auto ament_lint_common diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 7a7209a5..6a48dc0b 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -2,49 +2,58 @@ Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + Test plugin. - + + Test plugin. + + + Test plugin. + + + Test plugin. + + Test plugin. \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp index dd264156..5290e59d 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -19,7 +19,7 @@ class ValueBroadcaster "forward_list", [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); - declare_parameter>("forward_list", std::vector{}); + // declare_parameter>("forward_list", std::vector{}); } void before_pairing( diff --git a/rmcs_ws/src/rmcs_core/src/controller/arm/arm_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/arm/arm_controller.cpp new file mode 100644 index 00000000..39c1992e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/arm/arm_controller.cpp @@ -0,0 +1,474 @@ +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/drag_teach.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/endian_promise.hpp" +#include "hardware/fsm/FSM.hpp" +#include "hardware/fsm/FSM_gold_l.hpp" +#include "hardware/fsm/FSM_gold_m.hpp" +#include "hardware/fsm/FSM_gold_r.hpp" +#include "hardware/fsm/FSM_sliver.hpp" +#include "hardware/fsm/FSM_storage_lb.hpp" +#include "hardware/fsm/FSM_storage_rb.hpp" +#include "hardware/fsm/FSM_up_stairs.hpp" +#include "hardware/fsm/FSM_walk.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::arm { +class ArmController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + ArmController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + // register_input("/referee/vision/custom", custom_controller); + + register_input("/arm/Joint6/theta", theta[5]); + register_input("/arm/Joint5/theta", theta[4]); + register_input("/arm/Joint4/theta", theta[3]); + register_input("/arm/Joint3/theta", theta[2]); + register_input("/arm/Joint2/theta", theta[1]); + register_input("/arm/Joint1/theta", theta[0]); + register_output("/arm/Joint6/target_theta", target_theta[5], nan); + register_output("/arm/Joint5/target_theta", target_theta[4], nan); + register_output("/arm/Joint4/target_theta", target_theta[3], nan); + register_output("/arm/Joint3/target_theta", target_theta[2], nan); + register_output("/arm/Joint2/target_theta", target_theta[1], nan); + register_output("/arm/Joint1/target_theta", target_theta[0], nan); + + register_input("/arm/Joint1/raw_angle", joint1_raw_angle); + register_output("/arm/Joint1/zero_point", joint1_zero_point, 14381); + + register_output("/arm/enable_flag", is_arm_enable, true); + register_output("/arm/pump/target_vel", arm_pump_target_vel, NAN); + register_output("/mine/pump/target_vel", mine_pump_target_vel, NAN); + register_output("/arm/pump/relay/CH", arm_pump_relay, 0b00000000); + publisher_ = + create_publisher("/engineer/joint/measure", 10); + subscription_ = this->create_subscription( + "/engineer/joint/control", 10, + [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) { + if (msg->data.size() == 6) { + *target_theta[0] = static_cast(msg->data[0]); + *target_theta[1] = -static_cast(msg->data[1]); + *target_theta[2] = -static_cast(msg->data[2]) + std::numbers::pi / 2; + *target_theta[3] = static_cast(msg->data[3]); + *target_theta[4] = static_cast(msg->data[4]); + *target_theta[5] = static_cast(msg->data[5]); + } + }); + *arm_mode = rmcs_msgs::ArmMode::None; + register_output("/arm/mode", arm_mode, rmcs_msgs::ArmMode::None); + } + void update() override { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + auto msg = std_msgs::msg::Float32MultiArray(); + msg.data = { + static_cast(*theta[0]), + -static_cast(*theta[1]), + -(static_cast(*theta[2] - std::numbers::pi / 2)), + static_cast(*theta[3]), + static_cast(*theta[4]), + static_cast(*theta[5])}; + publisher_->publish(msg); + if (keyboard.e) { + if (keyboard.ctrl && keyboard.shift) { + *joint1_zero_point = *joint1_raw_angle; + *target_theta[0] = 0.0; + } + } + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + + reset_motors(); + } else { + *is_arm_enable = true; + + if (switch_left == rmcs_msgs::Switch::UP && switch_right == rmcs_msgs::Switch::UP) { + *arm_mode = rmcs_msgs::ArmMode::DT7_Control_Position; + } else if ( + switch_left == rmcs_msgs::Switch::UP && switch_right == rmcs_msgs::Switch::MIDDLE) { + *arm_mode = rmcs_msgs::ArmMode::DT7_Control_Orientation; + } else if ( + switch_left == rmcs_msgs::Switch::DOWN + && switch_right == rmcs_msgs::Switch::MIDDLE) { + is_arm_pump_on = true; + is_mine_pump_on = true; + } else if ( + switch_left == rmcs_msgs::Switch::DOWN && switch_right == rmcs_msgs::Switch::UP) { + if (keyboard.a) { + if (!keyboard.ctrl && !keyboard.shift) { + is_arm_pump_on = true; + + } else if (keyboard.shift && !keyboard.ctrl) { + is_arm_pump_on = false; + } + } + if (keyboard.s) { + if (!keyboard.ctrl && !keyboard.shift) { + is_mine_pump_on = true; + + } else if (keyboard.shift && !keyboard.ctrl) { + is_mine_pump_on = false; + } + } + if (keyboard.z) { + is_arm_pump_on = true; + if (!keyboard.ctrl && !keyboard.shift) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Gold_Left; + fsm_gold_l.reset(); + } else if (keyboard.shift && !keyboard.ctrl) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Gold_Mid; + fsm_gold_m.reset(); + } else if (keyboard.ctrl && !keyboard.shift) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Gold_Right; + fsm_gold_r.reset(); + } + } + + if (keyboard.x) { + is_arm_pump_on = true; + *arm_mode = rmcs_msgs::ArmMode::Auto_Sliver; + fsm_sliver.reset(); + } + if (keyboard.g) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Walk; + fsm_walk.reset(); + } + if (keyboard.d) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Spin; + fsm_walk.reset(); + } + if (keyboard.b) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Up_Stairs; + fsm_up_stairs.reset(); + } + if(keyboard.q&&keyboard.shift){ + *arm_mode =rmcs_msgs::ArmMode::Auto_Down_Stairs; + //rest for fsm_down_stairs.reset() + } + if (keyboard.f) { + is_arm_pump_on = true; + is_mine_pump_on = true; + if (keyboard.shift && !keyboard.ctrl) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Storage_LB; + fsm_storage_lb.reset(); + } else if (keyboard.ctrl && !keyboard.shift) { + *arm_mode = rmcs_msgs::ArmMode::Auto_Storage_RB; + fsm_storage_rb.reset(); + } + } + + if (keyboard.r) { + if (!keyboard.ctrl && !keyboard.shift) { + *arm_mode = rmcs_msgs::ArmMode::Customer; + }; + } + + } else { + *arm_mode = rmcs_msgs::ArmMode::None; + is_arm_pump_on = false; + is_mine_pump_on = false; + } + switch (*arm_mode) { + using namespace rmcs_msgs; + case ArmMode::Auto_Gold_Left: execute_gold(fsm_gold_l); break; + case ArmMode::Auto_Gold_Mid: execute_gold(fsm_gold_m); break; + case ArmMode::Auto_Gold_Right: execute_gold(fsm_gold_r); break; + case ArmMode::Auto_Sliver: execute_sliver(fsm_sliver); break; + case ArmMode::DT7_Control_Position: execute_dt7_position(); break; + case ArmMode::DT7_Control_Orientation: execute_dt7_orientation(); break; + // case ArmMode::Customer: execute_customer(); break; + case ArmMode::Auto_Up_Stairs: execute_up_stairs(); break; + case ArmMode::Auto_Storage_LB: execute_storage(fsm_storage_lb); break; + case ArmMode::Auto_Storage_RB: execute_storage(fsm_storage_rb); break; + case ArmMode::Auto_Spin: + case ArmMode::Auto_Walk: execute_walk(); break; + + default: break; + } + pump_control(); + } + } + +private: + void execute_dt7_orientation() { + if (fabs(joystick_left_->y()) > 0.01) { + *target_theta[5] += 0.003 * joystick_left_->y(); + *target_theta[5] = std::clamp(*target_theta[5], -3.1415926, 3.1415926); + } + if (fabs(joystick_left_->x()) > 0.01) { + *target_theta[4] += 0.003 * joystick_left_->x(); + *target_theta[4] = std::clamp(*target_theta[4], -1.83532, 1.83532); + } + if (fabs(joystick_right_->y()) > 0.01) { + *target_theta[3] += 0.003 * joystick_right_->y(); + *target_theta[3] = std::clamp(*target_theta[3], -3.1415926, 3.1415926); + } + } + void execute_dt7_position() { + if (fabs(joystick_left_->x()) > 0.01) { + *target_theta[2] += 0.001 * joystick_left_->x(); + *target_theta[2] = std::clamp(*target_theta[2], -1.0472, 0.8727); + } + if (fabs(joystick_right_->x()) > 0.01) { + *target_theta[1] += 0.001 * joystick_right_->x(); + *target_theta[1] = std::clamp(*target_theta[1], -1.308, 1.16719); + } + if (fabs(joystick_left_->y()) > 0.01) { + *target_theta[0] += 0.001 * joystick_left_->y(); + *target_theta[0] = std::clamp(*target_theta[0], -3.1415926, 3.1415926); + } + } + template + void execute_gold(T& fsm_gold) { + auto keyboard = *keyboard_; + auto mouse = *mouse_; + + static auto last_state = Auto_Gold_State::Set_initial; + + if (fsm_gold.fsm_direction == initial_enter || mouse.right) { + fsm_gold.get_current_theta( + {*theta[0], *theta[1], *theta[2], *theta[3], *theta[4], *theta[5]}); + } + if (mouse.left || fsm_gold.fsm_direction == initial_enter) { + fsm_gold.fsm_direction = up; + } else if (mouse.right) { + fsm_gold.fsm_direction = down; + } + if (fsm_gold.fsm_direction == up) { + fsm_gold.processEvent(Auto_Gold_Event::Up); + } else if (fsm_gold.fsm_direction == down) { + fsm_gold.processEvent(Auto_Gold_Event::Down); + } + if (fsm_gold.getState() != last_state) { + fsm_gold.fsm_direction = 0; + } + last_state = fsm_gold.getState(); + *target_theta[5] = fsm_gold.get_result()[5]; + *target_theta[4] = fsm_gold.get_result()[4]; + *target_theta[3] = fsm_gold.get_result()[3]; + *target_theta[2] = fsm_gold.get_result()[2]; + *target_theta[1] = fsm_gold.get_result()[1]; + *target_theta[0] = fsm_gold.get_result()[0]; + } + template + void execute_sliver(T& fsm_sliver) { + auto keyboard = *keyboard_; + auto mouse = *mouse_; + static auto last_state = Auto_Sliver_State::Set_initial; + + if (fsm_sliver.fsm_direction == initial_enter || mouse.right) { + fsm_sliver.get_current_theta( + {*theta[0], *theta[1], *theta[2], *theta[3], *theta[4], *theta[5]}); + } + if (mouse.left || fsm_sliver.fsm_direction == initial_enter) + fsm_sliver.fsm_direction = up; + else if (mouse.right) + fsm_sliver.fsm_direction = down; + + if (fsm_sliver.fsm_direction == up) { + fsm_sliver.processEvent(Auto_Sliver_Event::Up); + } else if (fsm_sliver.fsm_direction == down) { + fsm_sliver.processEvent(Auto_Sliver_Event::Down); + } + + if (fsm_sliver.getState() != last_state) { + fsm_sliver.fsm_direction = 0; + } + last_state = fsm_sliver.getState(); + *target_theta[5] = fsm_sliver.get_result()[5]; + *target_theta[4] = fsm_sliver.get_result()[4]; + *target_theta[3] = fsm_sliver.get_result()[3]; + *target_theta[2] = fsm_sliver.get_result()[2]; + *target_theta[1] = fsm_sliver.get_result()[1]; + *target_theta[0] = fsm_sliver.get_result()[0]; + } + void execute_walk() { + auto keyboard = *keyboard_; + if (fsm_walk.fsm_direction == initial_enter) { + fsm_walk.get_current_theta( + {*theta[0], *theta[1], *theta[2], *theta[3], *theta[4], *theta[5]}); + fsm_walk.fsm_direction = up; + } + if (fsm_walk.fsm_direction == up) { + fsm_walk.processEvent(Auto_Walk_Event::Up); + } + *target_theta[5] = fsm_walk.get_result()[5]; + *target_theta[4] = fsm_walk.get_result()[4]; + *target_theta[3] = fsm_walk.get_result()[3]; + *target_theta[2] = fsm_walk.get_result()[2]; + *target_theta[1] = fsm_walk.get_result()[1]; + *target_theta[0] = fsm_walk.get_result()[0]; + } + + void execute_customer() { + + // std::memcpy(customer_theta, custom_controller->begin() + 1, sizeof(customer_theta)); + double customer_[6]; + for (int i = 0; i < 6; ++i) { + customer_[i] = customer_theta[i]; + } + *target_theta[5] = customer_[5]; + *target_theta[4] = customer_[4]; + *target_theta[3] = customer_[3]; + *target_theta[2] = customer_[2]; + *target_theta[1] = customer_[1]; + *target_theta[0] = customer_[0]; + } + void execute_up_stairs() { + auto keyboard = *keyboard_; + auto mouse = *mouse_; + + static auto last_state = Auto_Up_Stairs_State::Leg_Initial; + + if (fsm_up_stairs.fsm_direction == initial_enter + || (keyboard.ctrl && fsm_up_stairs.getState() == Auto_Up_Stairs_State::Leg_Lift)) { + fsm_up_stairs.fsm_direction = up; + fsm_up_stairs.get_current_theta( + {*theta[0], *theta[1], *theta[2], *theta[3], *theta[4], *theta[5]}); + } + + if (fsm_up_stairs.fsm_direction == up) { + fsm_up_stairs.processEvent(Auto_Up_Stairs_Event::Up); + } else if (fsm_up_stairs.fsm_direction == down) { + fsm_up_stairs.processEvent(Auto_Up_Stairs_Event::Down); + } + if (fsm_up_stairs.getState() != last_state) { + fsm_up_stairs.fsm_direction = 0; + } + last_state = fsm_up_stairs.getState(); + *target_theta[5] = fsm_up_stairs.get_result()[5]; + *target_theta[4] = fsm_up_stairs.get_result()[4]; + *target_theta[3] = fsm_up_stairs.get_result()[3]; + *target_theta[2] = fsm_up_stairs.get_result()[2]; + *target_theta[1] = fsm_up_stairs.get_result()[1]; + *target_theta[0] = fsm_up_stairs.get_result()[0]; + } + template + void execute_storage(T& fsm) { + auto keyboard = *keyboard_; + if (fsm.fsm_direction == initial_enter) { + fsm.get_current_theta( + {*theta[0], *theta[1], *theta[2], *theta[3], *theta[4], *theta[5]}); + fsm.fsm_direction = up; + } + if (fsm.fsm_direction == up) { + fsm.processEvent(Auto_Storage_Event::Up); + } + if (fsm.get_first_trajectory_result()) { + is_arm_pump_on = false; + is_mine_pump_on = true; + } + *target_theta[5] = fsm.get_result()[5]; + *target_theta[4] = fsm.get_result()[4]; + *target_theta[3] = fsm.get_result()[3]; + *target_theta[2] = fsm.get_result()[2]; + *target_theta[1] = fsm.get_result()[1]; + *target_theta[0] = fsm.get_result()[0]; + } + + void pump_control() { + if (is_arm_pump_on) { + *arm_pump_target_vel = 5300 * std::numbers::pi / 30.0; + *arm_pump_relay = 0b00000000; + } else { + *arm_pump_target_vel = 0.0; + *arm_pump_relay = 0b11111111; + } + if (is_mine_pump_on) { + *mine_pump_target_vel = 5300 * std::numbers::pi / 30.0; + + } else { + *mine_pump_target_vel = 0.0; + } + } + void reset_motors() { + *is_arm_enable = false; + *target_theta[5] = *theta[5]; + *target_theta[4] = *theta[4]; + *target_theta[3] = *theta[3]; + *target_theta[2] = *theta[2]; + *target_theta[1] = *theta[1]; + *target_theta[0] = *theta[0]; + *arm_pump_target_vel = NAN; + *mine_pump_target_vel = NAN; + } + bool is_auto_exchange = false; + // auto_mode_Fsm + Auto_Gold_Left fsm_gold_l; + Auto_Gold_Mid fsm_gold_m; + Auto_Gold_Right fsm_gold_r; + Auto_Sliver fsm_sliver; + Auto_Set_Walk_Arm fsm_walk; + Auto_Up_Stairs fsm_up_stairs; + Auto_Storage_LB fsm_storage_lb; + Auto_Storage_RB fsm_storage_rb; + + OutputInterface arm_mode; + rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr publisher_; + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + float customer_theta[6]; + // InputInterface> custom_controller; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + OutputInterface is_arm_enable; + InputInterface theta[6]; // motor_current_angle + OutputInterface target_theta[6]; + + InputInterface joint1_raw_angle; + OutputInterface joint1_zero_point; + + OutputInterface arm_pump_target_vel; + OutputInterface mine_pump_target_vel; + OutputInterface arm_pump_relay; + + bool is_arm_pump_on = false; + bool is_mine_pump_on = false; +}; +} // namespace rmcs_core::controller::arm +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::arm::ArmController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index b0528d0f..bdf8c2a6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -1,23 +1,32 @@ +#include "controller/pid/pid_calculator.hpp" +#include "hardware/device/trajectory.hpp" +#include "rmcs_msgs/arm_mode.hpp" +#include +#include +#include +#include #include +#include +#include +#include #include #include #include +#include #include #include #include #include - namespace rmcs_core::controller::chassis { - -class ChassisController +class Chassis_Controller : public rmcs_executor::Component , public rclcpp::Node { public: - ChassisController() + Chassis_Controller() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(1.0, 0.0, 0.0) { register_input("/remote/joystick/right", joystick_right_); register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -26,145 +35,255 @@ class ChassisController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); - register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); - - register_input("/chassis/supercap/voltage", supercap_voltage_); - register_input("/chassis/supercap/enabled", supercap_enabled_); + // register_input("yaw_imu_velocity", yaw_imu_velocity); + register_input("yaw_imu_angle", yaw_imu_angle); + register_input("/arm/Joint1/theta", joint1_theta); - register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); - register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_input("/arm/mode", arm_mode); - register_output("/chassis/control_mode", mode_); - register_output("/chassis/control_move", move_); - - register_output("/chassis/supercap/control_enable", supercap_control_enabled_, false); register_output( - "/chassis/supercap/control_power_limit", supercap_control_power_limit_, 0.0); + "/chassis/big_yaw/target_angle_error", chassis_big_yaw_target_angle_error, NAN); + register_input("/chassis/big_yaw/angle", chassis_big_yaw_angle); register_output("/chassis/control_power_limit", chassis_control_power_limit_, 0.0); + register_output("/chassis/control_velocity", chassis_control_velocity_); - register_output( - "/chassis/supercap/voltage/control_line", supercap_voltage_control_line_, - supercap_voltage_control_line); - register_output( - "/chassis/supercap/voltage/base_line", supercap_voltage_base_line_, - supercap_voltage_base_line); - register_output( - "/chassis/supercap/voltage/dead_line", supercap_voltage_dead_line_, - supercap_voltage_dead_line); + register_input("/steering/power_meter/power", chassis_power_); + // register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); + register_output("/move_speed_limit", speed_limit_, NAN); } - void update() override { - using namespace rmcs_msgs; - auto switch_right = *switch_right_; auto switch_left = *switch_left_; + auto mouse = *mouse_; auto keyboard = *keyboard_; + *speed_limit_ = move_speed_limit; + Eigen::Vector2d move_; + double angular_velocity = 0.0; + using namespace rmcs_msgs; - do { + if (!initial_check_done_) { + reset_motor(); + yaw_control_theta_in_IMU = *yaw_imu_angle; + is_yaw_imu_control = true; + if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + initial_check_done_ = true; + } + } else { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - auto mode = *mode_; - if (switch_left != Switch::DOWN) { - if ((last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) - || (!last_keyboard_.c && keyboard.c)) { - mode = mode == rmcs_msgs::ChassisMode::SPIN ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::SPIN; - } else if (!last_keyboard_.x && keyboard.x) { - mode = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::LAUNCH_RAMP; - } else if (!last_keyboard_.z && keyboard.z) { - mode = mode == rmcs_msgs::ChassisMode::STEP_DOWN - ? rmcs_msgs::ChassisMode::AUTO - : rmcs_msgs::ChassisMode::STEP_DOWN; + } else { + mode_selection(); + + switch (chassis_mode) { + case rmcs_msgs::ChassisMode::Flow: { + double chassis_theta = *chassis_big_yaw_angle; + angular_velocity = + std::clamp(following_velocity_controller_.update(chassis_theta), + -1.0, 1.0); + break; + } + case rmcs_msgs::ChassisMode::SPIN: { + angular_velocity = 5; + yaw_control_theta_in_IMU += joystick_right_->y() * 0.002; + break; + } + case rmcs_msgs::ChassisMode::Up_Stairs: { + is_yaw_imu_control = false; + yaw_set_theta_in_YawFreeMode = 0.0; + move_speed_limit = 1.5; + move_ = *joystick_left_; + break; + } + default: break; } - *mode_ = mode; + Eigen::Rotation2D rotation(*chassis_big_yaw_angle + *joint1_theta); + // move_ = rotation * (*joystick_left_); + move_ = (*joystick_left_); + angular_velocity = joystick_right_->y()*angular_velocity_limit; + // RCLCPP_INFO( + // this->get_logger(), "joystick x:%f y:%f angular_velocity:%f", move_.x(), + // move_.y(), angular_velocity); + chassis_control_velocity_->vector << (move_ * move_speed_limit), angular_velocity; + // yaw_control(); + update_virtual_buffer_energy(); + update_control_power_limit(); } + } + } - auto keyboard_move = - Eigen::Vector2d{0.5 * (keyboard.w - keyboard.s), 0.5 * (keyboard.a - keyboard.d)}; - auto move = - (Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move)) - .eval(); - if (move.norm() > 1) - move.normalize(); - *move_ = {move.x(), move.y(), 0}; - - if (!supercap_switch_cooling_) { - bool enable = keyboard_->shift; - if (*supercap_control_enabled_ != enable) { - *supercap_control_enabled_ = enable; - supercap_switch_cooling_ = 500; +private: + void mode_selection() { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + using namespace rmcs_msgs; + if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) { + chassis_mode = ChassisMode::Flow; + is_yaw_imu_control = true; + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) { + chassis_mode = ChassisMode::Flow; + is_yaw_imu_control = true; + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) { + chassis_mode = ChassisMode::SPIN; + is_yaw_imu_control = true; + } else if (switch_left == Switch::DOWN && switch_right == Switch::UP) { + if (keyboard.c) { + if (!keyboard.shift && !keyboard.ctrl) { + move_speed_limit = 4.5; + } + if (keyboard.shift && !keyboard.ctrl) { + move_speed_limit = 2.5; + } + if (!keyboard.shift && keyboard.ctrl) { + move_speed_limit = 0.8; } - } else { - --supercap_switch_cooling_; } - - double power_limit_after_buffer_energy_closed_loop = - *chassis_power_limit_referee_ - * std::clamp( - (*chassis_buffer_energy_referee_ - buffer_energy_dead_line) - / (buffer_energy_base_line - buffer_energy_dead_line), - 0.0, 1.0) - + excess_power_limit - * std::clamp( - (*chassis_buffer_energy_referee_ - buffer_energy_base_line) - / (buffer_energy_control_line - buffer_energy_base_line), - 0.0, 1.0); - *supercap_control_power_limit_ = power_limit_after_buffer_energy_closed_loop; - - if (*supercap_control_enabled_ && *supercap_enabled_) { - double supercap_power_limit = mode == rmcs_msgs::ChassisMode::LAUNCH_RAMP - ? 250.0 - : *chassis_power_limit_referee_ + 80.0; - *chassis_control_power_limit_ = - *chassis_power_limit_referee_ - * std::clamp( - (*supercap_voltage_ - supercap_voltage_dead_line) - / (supercap_voltage_base_line - supercap_voltage_dead_line), - 0.0, 1.0) - + (supercap_power_limit - *chassis_power_limit_referee_) - * std::clamp( - (*supercap_voltage_ - supercap_voltage_base_line) - / (supercap_voltage_control_line - supercap_voltage_base_line), - 0.0, 1.0); - } else { - *chassis_control_power_limit_ = power_limit_after_buffer_energy_closed_loop; + if (keyboard.q) { + if (!keyboard.shift && !keyboard.ctrl) { + chassis_mode = rmcs_msgs::ChassisMode::Flow; + is_yaw_imu_control = true; + } } + if (keyboard.d) { + chassis_mode = rmcs_msgs::ChassisMode::SPIN; + is_yaw_imu_control = true; + } + if (keyboard.b) { + chassis_mode = rmcs_msgs::ChassisMode::Up_Stairs; + } + // execute right change according to arm_mode + if (last_arm_mode != *arm_mode) { + + if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left + || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Mid + || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right + || *arm_mode == rmcs_msgs::ArmMode::Auto_Sliver + || *arm_mode == rmcs_msgs::ArmMode::Auto_Ground + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_LB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_RB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Extract + || *arm_mode == rmcs_msgs::ArmMode::Customer + || *arm_mode == rmcs_msgs::ArmMode::Auto_Up_Stairs) { + move_speed_limit = 1.6; + is_yaw_imu_control = false; + if (*arm_mode == rmcs_msgs::ArmMode::Customer) { + chassis_mode = rmcs_msgs::ChassisMode::Yaw_Free; + yaw_set_theta_in_YawFreeMode = 0.0; + } else { + chassis_mode = ChassisMode::Yaw_Free; + if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left) { + yaw_set_theta_in_YawFreeMode = std::numbers::pi / 2.0; + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right) { + yaw_set_theta_in_YawFreeMode = -std::numbers::pi / 2.0; + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Mid) { + yaw_set_theta_in_YawFreeMode = 0.0; + + } else if ( + *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_LB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_RB) { + if (last_arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left) { + yaw_set_theta_in_YawFreeMode = std::numbers::pi / 2.0; + } else if (last_arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right) { + yaw_set_theta_in_YawFreeMode = -std::numbers::pi / 2.0; + } else { + yaw_set_theta_in_YawFreeMode = 0.0; + } + + } else { + yaw_set_theta_in_YawFreeMode = 0.0; + } + } + yaw_trajectory_controller.set_start_point({*chassis_big_yaw_angle}) + .set_total_step(1400) + .set_end_point({yaw_set_theta_in_YawFreeMode}) + .reset(); + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Walk) { + move_speed_limit = 4.5; + chassis_mode = rmcs_msgs::ChassisMode::Flow; + is_yaw_imu_control = true; + } + } + } else { + chassis_mode = rmcs_msgs::ChassisMode::None; + } + if (last_chassis_mode != chassis_mode) { + if (last_chassis_mode == rmcs_msgs::ChassisMode::SPIN) { + move_speed_limit = 4.5; + } + if (last_chassis_mode == rmcs_msgs::ChassisMode::Yaw_Free + || last_chassis_mode == rmcs_msgs::ChassisMode::Up_Stairs) { + if (last_is_yaw_imu_control != is_yaw_imu_control) { + yaw_control_theta_in_IMU = *yaw_imu_angle; + } + } + } + last_chassis_mode = chassis_mode; + last_arm_mode = *arm_mode; + last_is_yaw_imu_control = is_yaw_imu_control; + } - } while (false); - - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + void yaw_control() { + if (chassis_mode != rmcs_msgs::ChassisMode::Up_Stairs) { + yaw_control_theta_in_IMU += joystick_right_->y() * 0.002; + } + if (is_yaw_imu_control) { + *chassis_big_yaw_target_angle_error = + normalizeAngle(yaw_control_theta_in_IMU - *yaw_imu_angle); + } else { + std::array result = yaw_trajectory_controller.trajectory(); + *chassis_big_yaw_target_angle_error = + normalizeAngle(result[0] - *chassis_big_yaw_angle); + } + } + void update_virtual_buffer_energy() { + constexpr double dt = 1e-3; + virtual_buffer_energy_ += dt * (chassis_power_limit_expected_ - *chassis_power_); + virtual_buffer_energy_ = std::clamp( + virtual_buffer_energy_, 0.0, + std::min(chassis_buffer_energy_referee_, virtual_buffer_energy_limit_)); } - void reset_all_controls() { - *mode_ = rmcs_msgs::ChassisMode::AUTO; + void update_control_power_limit() { + double power_limit; - *supercap_control_enabled_ = false; - *supercap_control_power_limit_ = 0.0; - *chassis_control_power_limit_ = 0.0; - } + power_limit = chassis_power_limit_referee_; + chassis_power_limit_expected_ = power_limit; -private: - // Maximum excess power when buffer energy is sufficient. - static constexpr double excess_power_limit = 35; + constexpr double excess_power_limit = 15; - // power_limit_after_buffer_energy_closed_loop = - static constexpr double buffer_energy_control_line = 120; // = referee + excess - static constexpr double buffer_energy_base_line = 50; // = referee - static constexpr double buffer_energy_dead_line = 0; // = 0 + power_limit += excess_power_limit; + power_limit *= virtual_buffer_energy_ / virtual_buffer_energy_limit_; - // chassis_control_power = - static constexpr double supercap_voltage_control_line = 13.5; // = supercap - static constexpr double supercap_voltage_base_line = 11.5; // = referee - static constexpr double supercap_voltage_dead_line = 10.5; // = 0 + *chassis_control_power_limit_ = power_limit; // + } + void reset_motor() { + *chassis_big_yaw_target_angle_error = NAN; + *chassis_control_velocity_ = {nan, nan, nan}; + *chassis_control_power_limit_ = 0.0; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; // + } + static double normalizeAngle(double angle) { + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + + static constexpr double inf = std::numeric_limits::infinity(); + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + InputInterface arm_mode; + rmcs_msgs::ArmMode last_arm_mode; + pid::PidCalculator following_velocity_controller_; + OutputInterface speed_limit_; + double move_speed_limit = 4.5; + double angular_velocity_limit = 16.0; // m/s + constexpr static double chassis_power_limit_referee_ = 120.0f; + InputInterface joystick_right_; InputInterface joystick_left_; InputInterface switch_right_; @@ -173,34 +292,30 @@ class ChassisController InputInterface mouse_; InputInterface keyboard_; - rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - - InputInterface gimbal_yaw_angle_; + bool is_yaw_imu_control = false; + bool last_is_yaw_imu_control = false; + InputInterface yaw_imu_angle; + InputInterface joint1_theta; + double yaw_control_theta_in_IMU = NAN; + double yaw_set_theta_in_YawFreeMode = NAN; + hardware::device::Trajectory yaw_trajectory_controller; - OutputInterface mode_; - OutputInterface move_; - - InputInterface supercap_voltage_; - InputInterface supercap_enabled_; - - InputInterface chassis_power_limit_referee_; - InputInterface chassis_buffer_energy_referee_; - - int supercap_switch_cooling_ = 0; - OutputInterface supercap_control_enabled_; - OutputInterface supercap_control_power_limit_; + OutputInterface chassis_big_yaw_target_angle_error; + InputInterface chassis_big_yaw_angle; + rmcs_msgs::ChassisMode chassis_mode = rmcs_msgs::ChassisMode::None; + rmcs_msgs::ChassisMode last_chassis_mode = rmcs_msgs::ChassisMode::None; OutputInterface chassis_control_power_limit_; - - OutputInterface supercap_voltage_control_line_; - OutputInterface supercap_voltage_base_line_; - OutputInterface supercap_voltage_dead_line_; + OutputInterface chassis_control_velocity_; + double virtual_buffer_energy_; + static constexpr double virtual_buffer_energy_limit_ = 30.0; + double chassis_power_limit_expected_; + InputInterface chassis_power_; + // InputInterface chassis_buffer_energy_referee_; + double chassis_buffer_energy_referee_ = 60.0f; + bool initial_check_done_ = false; }; - } // namespace rmcs_core::controller::chassis - #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::Chassis_Controller, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_leg_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_leg_controller.cpp new file mode 100644 index 00000000..3051ea62 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_leg_controller.cpp @@ -0,0 +1,634 @@ +#include "controller/pid/pid_calculator.hpp" +#include "hardware/device/trajectory.hpp" +#include +#include "rmcs_msgs/arm_mode.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::controller::chassis { +class Chassis_and_LegController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Chassis_and_LegController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(0.46, 0.00, 0.089) + , leg_lf_error_pid_controller(240.0, 0.0, 5.0) + , leg_lf_velocity_pid_controller(2.0, 0.0, 0.2) + , leg_rf_error_pid_controller(240.0, 0.0, 5.0) + , leg_rf_velocity_pid_controller(2.0, 0.0, 0.2) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_output("/chassis_and_leg/enable_flag", is_chassis_and_leg_enable, true); + register_input("/steering/steering/lf/angle", steering_lf_angle); + register_input("/steering/steering/lb/angle", steering_lb_angle); + register_input("/steering/steering/rb/angle", steering_rb_angle); + register_input("/steering/steering/rf/angle", steering_rf_angle); + register_output( + "/steering/steering/lf/target_angle_error", steering_lf_target_angle_error, NAN); + register_output( + "/steering/steering/lb/target_angle_error", steering_lb_target_angle_error, NAN); + register_output( + "/steering/steering/rb/target_angle_error", steering_rb_target_angle_error, NAN); + register_output( + "/steering/steering/rf/target_angle_error", steering_rf_target_angle_error, NAN); + + register_output("/steering/wheel/lf/target_vel", steering_wheel_lf_target_vel, NAN); + register_output("/steering/wheel/lb/target_vel", steering_wheel_lb_target_vel, NAN); + register_output("/steering/wheel/rb/target_vel", steering_wheel_rb_target_vel, NAN); + register_output("/steering/wheel/rf/target_vel", steering_wheel_rf_target_vel, NAN); + register_output("/leg/omni/l/target_vel", omni_l_target_vel, NAN); + register_output("/leg/omni/r/target_vel", omni_r_target_vel, NAN); + + register_input("/leg/encoder/lf/angle", theta_lf); + register_input("/leg/encoder/lb/angle", theta_lb); + register_input("/leg/encoder/rb/angle", theta_rb); + register_input("/leg/encoder/rf/angle", theta_rf); + register_output("/leg/joint/lf/control_torque", leg_joint_lf_control_torque, NAN); + register_output("/leg/joint/lb/target_theta", leg_lb_target_theta, NAN); + register_output("/leg/joint/rb/target_theta", leg_rb_target_theta, NAN); + register_output("/leg/joint/rf/control_torque", leg_joint_rf_control_torque, NAN); + + register_input("/leg/joint/lf/velocity", leg_joint_lf_velocity); + register_input("/leg/joint/rf/velocity", leg_joint_rf_velocity); + register_input("/leg/joint/lf/torque", leg_joint_lf_torque); + register_input("/leg/joint/rf/torque", leg_joint_rf_torque); + + register_input("yaw_imu_velocity", yaw_imu_velocity); + register_input("yaw_imu_angle", yaw_imu_angle); + register_input("/arm/Joint1/theta", joint1_theta); + + register_input("/arm/mode", arm_mode); + + register_output( + "/chassis/big_yaw/target_angle_error", chassis_big_yaw_target_angle_error, NAN); + register_input("/chassis/big_yaw/angle", chassis_big_yaw_angle); + + std::array four_wheel_angle = leg_inverse_kinematic(237.0, 221.0, false, false); + four_wheel_trajectory + .set_end_point( + {four_wheel_angle[0], four_wheel_angle[1], four_wheel_angle[1], four_wheel_angle[0], + 0, 0}) + .set_total_step(500.0); + std::array six_wheel_angle = leg_inverse_kinematic(250.0, 221.0, false, false); + six_wheel_trajectory + .set_end_point( + {six_wheel_angle[0], six_wheel_angle[1], six_wheel_angle[1], six_wheel_angle[0], 0, + 0}) + .set_total_step(500.0); + + up_stairs_initial.set_end_point({1.700270, 1.680, 1.680, 1.700270, 0, 0}) + .set_total_step(2000); + up_stairs_leg_press.set_end_point({0.506814, 0.995241, 0.995241, 0.466814, 0, 0}) + .set_total_step(1000); + up_stairs_leg_lift + .set_end_point({1.2193598767, 1.261730456, 1.261730456, 1.2193598767, 0, 0}) + .set_total_step(3100); + } + void update() override { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + using namespace rmcs_msgs; + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + *is_chassis_and_leg_enable = false; + reset_motor(); + yaw_control_theta = *yaw_imu_angle; + is_yaw_imu_control = true; + is_leg_forward_joint_torque_control = false; + leg_mode = rmcs_msgs::LegMode::None; + } else { + + *is_chassis_and_leg_enable = true; + mode_selection(); + switch (chassis_mode) { + case rmcs_msgs::ChassisMode::Flow: { + double chassis_theta = *chassis_big_yaw_angle; + double spin_speed = + std::clamp(following_velocity_controller_.update(chassis_theta), -1.0, 1.0); + Eigen::Rotation2D rotation(chassis_theta + *joint1_theta); + Eigen::Vector2d move_ = *joystick_left_; + yaw_control_theta += joystick_right_->y() * 0.002; + yaw_control_theta = normalizeAngle(yaw_control_theta); + if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { + omniwheel_control(Eigen::Vector2d{NAN, NAN}); + } else { + omniwheel_control(move_); + } + steering_control(move_, spin_speed); + break; + } + case rmcs_msgs::ChassisMode::SPIN: { + // leg_mode = rmcs_msgs::LegMode::Four_Wheel; + double spin_speed = 0.8; + speed_limit = 5.0; + Eigen::Rotation2D rotation(*chassis_big_yaw_angle + *joint1_theta); + Eigen::Vector2d move_ = rotation * (*joystick_left_); + yaw_control_theta += joystick_right_->y() * 0.002; + steering_control(move_, spin_speed); + omniwheel_control(Eigen::Vector2d{NAN, NAN}); + + break; + + } + case rmcs_msgs::ChassisMode::Up_Stairs: { + is_yaw_imu_control = false; + yaw_set_theta_in_YawFreeMode = 0.0; + speed_limit = 1.5; + Eigen::Vector2d move_ = *joystick_left_; + steering_control(move_, joystick_right_->y()); + omniwheel_control(move_); + break; + } + case rmcs_msgs::ChassisMode::Yaw_Free: { + Eigen::Rotation2D rotation(*chassis_big_yaw_angle + *joint1_theta); + Eigen::Vector2d move_ = rotation * (*joystick_left_); + steering_control(move_, joystick_right_->y() / 1.5); + omniwheel_control(move_); + break; + } + default: break; + } + yaw_control(); + leg_control(); + } + } + +private: + void mode_selection() { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + using namespace rmcs_msgs; + if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) { + chassis_mode = ChassisMode::Flow; + is_yaw_imu_control = true; + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) { + chassis_mode = ChassisMode::Flow; + is_yaw_imu_control = true; + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) { + chassis_mode = ChassisMode::SPIN; + is_yaw_imu_control = true; + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + + } else if (switch_left == Switch::DOWN && switch_right == Switch::UP) { + if (keyboard.c) { + if (!keyboard.shift && !keyboard.ctrl) { + speed_limit = 4.5; + } + if (keyboard.shift && !keyboard.ctrl) { + speed_limit = 2.5; + } + if (!keyboard.shift && keyboard.ctrl) { + speed_limit = 0.8; + } + } + if (keyboard.v) { + if (!keyboard.shift && !keyboard.ctrl) { + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + } + if (keyboard.shift && !keyboard.ctrl) { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + } + } + if (keyboard.q) { + if (!keyboard.shift && !keyboard.ctrl) { + chassis_mode = rmcs_msgs::ChassisMode::Flow; + is_yaw_imu_control = true; + } + + } + if (keyboard.d) { + chassis_mode = rmcs_msgs::ChassisMode::SPIN; + is_yaw_imu_control = true; + } + if (keyboard.b) { + chassis_mode = rmcs_msgs::ChassisMode::Up_Stairs; + leg_mode = rmcs_msgs::LegMode::Up_Stairs; + up_stairs_is_leg_press = false; + up_stairs_is_leg_lift = false; + up_stairs_initial.reset(); + up_stairs_leg_press.reset(); + up_stairs_leg_lift.reset(); + up_stairs_initial.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + } + // execute right change according to arm_mode + if (last_arm_mode != *arm_mode) { + + if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left + || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Mid + || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right + || *arm_mode == rmcs_msgs::ArmMode::Auto_Sliver + || *arm_mode == rmcs_msgs::ArmMode::Auto_Ground + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_LB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_RB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Extract + || *arm_mode == rmcs_msgs::ArmMode::Customer + || *arm_mode == rmcs_msgs::ArmMode::Auto_Up_Stairs) { + speed_limit = 1.6; + is_yaw_imu_control = false; + if (*arm_mode == rmcs_msgs::ArmMode::Customer) { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + chassis_mode = rmcs_msgs::ChassisMode::Yaw_Free; + yaw_set_theta_in_YawFreeMode = 0.0; + } else { + chassis_mode = ChassisMode::Yaw_Free; + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + + if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left) { + yaw_set_theta_in_YawFreeMode = std::numbers::pi / 2.0; + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right) { + yaw_set_theta_in_YawFreeMode = -std::numbers::pi / 2.0; + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Mid) { + yaw_set_theta_in_YawFreeMode = 0.0; + + } else if ( + *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_LB + || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_RB) { + if (last_arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left) { + yaw_set_theta_in_YawFreeMode = std::numbers::pi / 2.0; + } else if (last_arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right) { + yaw_set_theta_in_YawFreeMode = -std::numbers::pi / 2.0; + } else { + yaw_set_theta_in_YawFreeMode = 0.0; + } + + } else { + yaw_set_theta_in_YawFreeMode = 0.0; + } + } + yaw_trajectory_controller.set_start_point({*chassis_big_yaw_angle}) + .set_total_step(1400) + .set_end_point({yaw_set_theta_in_YawFreeMode}) + .reset(); + } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Walk) { + speed_limit = 4.5; + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + chassis_mode = rmcs_msgs::ChassisMode::Flow; + is_yaw_imu_control = true; + } + } + } else { + chassis_mode = rmcs_msgs::ChassisMode::None; + leg_mode = rmcs_msgs::LegMode::None; + } + + if (last_leg_mode != leg_mode) { + if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { + four_wheel_trajectory.reset(); + four_wheel_trajectory.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + + } else if (leg_mode == rmcs_msgs::LegMode::Six_Wheel) { + six_wheel_trajectory.reset(); + six_wheel_trajectory.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + } + } + if (last_chassis_mode != chassis_mode) { + if (last_chassis_mode == rmcs_msgs::ChassisMode::SPIN) { + speed_limit = 4.5; + } + if (last_chassis_mode == rmcs_msgs::ChassisMode::Yaw_Free + || last_chassis_mode == rmcs_msgs::ChassisMode::Up_Stairs) { + if (last_is_yaw_imu_control != is_yaw_imu_control) { + yaw_control_theta = *yaw_imu_angle; + } + } + } + last_chassis_mode = chassis_mode; + last_arm_mode = *arm_mode; + last_leg_mode = leg_mode; + last_is_yaw_imu_control = is_yaw_imu_control; + } + + void leg_control() { + + static std::array result; + + if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { + is_leg_forward_joint_torque_control = false; + result = four_wheel_trajectory.trajectory(); + + } else if (leg_mode == rmcs_msgs::LegMode::Six_Wheel) { + if (!six_wheel_trajectory.get_complete()) { + is_leg_forward_joint_torque_control = false; + result = six_wheel_trajectory.trajectory(); + } else { + is_leg_forward_joint_torque_control = false; + result = six_wheel_trajectory.trajectory(); + if (*theta_lf < std::numbers::pi / 2.0 || *theta_rf < std::numbers::pi / 2.0) { + result[0] = NAN; + result[3] = NAN; + } + } + } else if (leg_mode == rmcs_msgs::LegMode::Up_Stairs) { + is_leg_forward_joint_torque_control = false; + if (!up_stairs_initial.get_complete()) { + result = up_stairs_initial.trajectory(); + up_stairs_leg_press.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + up_stairs_is_leg_press = false; + } else { + if (keyboard_->shift) { + up_stairs_is_leg_press = true; + } + if (keyboard_->ctrl) { + up_stairs_is_leg_lift = true; + } else if (up_stairs_is_leg_press) { + if (!up_stairs_leg_press.get_complete()) { + result = up_stairs_leg_press.trajectory(); + up_stairs_leg_lift.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + } else if (up_stairs_is_leg_lift) { + result = up_stairs_leg_lift.trajectory(); + if (up_stairs_leg_lift.get_complete()) { + result[0] = 0.8; + result[3] = 0.8; + } + } + } + } + } else if (leg_mode == rmcs_msgs::LegMode::None) { + is_leg_forward_joint_torque_control = false; + result[0] = *theta_lf; + result[1] = *theta_lb; + result[2] = *theta_rb; + result[3] = *theta_rf; + } + leg_joint_controller(result[0], result[1], result[2], result[3]); + } + + void leg_joint_controller(double lf, double lb, double rb, double rf) { + if (is_leg_forward_joint_torque_control) { + set_leg_forward_joint_torque(lf, rf); + *leg_lb_target_theta = lb; + *leg_rb_target_theta = rb; + + } else { + leg_lf_target_theta = lf; + leg_rf_target_theta = rf; + *leg_lb_target_theta = lb; + *leg_rb_target_theta = rb; + double diff = (*theta_rf); + double leg_joint_lf_control_vel = + leg_lf_error_pid_controller.update(normalizeAngle(leg_lf_target_theta - *theta_lf)); + double leg_joint_rf_control_vel = + leg_rf_error_pid_controller.update(normalizeAngle(leg_rf_target_theta - *theta_rf)); + set_leg_forward_joint_torque( + leg_lf_velocity_pid_controller.update( + leg_joint_lf_control_vel - *leg_joint_lf_velocity), + leg_rf_velocity_pid_controller.update( + leg_joint_rf_control_vel - *leg_joint_rf_velocity)); + } + }; + void set_leg_forward_joint_torque(double lf, double rf) { + *leg_joint_lf_control_torque = lf; + *leg_joint_rf_control_torque = rf; + } + static std::array leg_inverse_kinematic( + double f_x, double b_x, bool is_front_ecd_obtuse, bool is_back_ecd_obtuse) { + constexpr double link1 = 240.0f, link2 = 120.0f, link3 = 160.0f; + double theta_f, theta_b; + double theta_b_ = asin(b_x / link1); + if (is_back_ecd_obtuse) { + theta_b = std::numbers::pi - theta_b_; + } else { + theta_b = theta_b_; + } + double x_link2 = link2 * sin(5.0 * std::numbers::pi / 6.0 - theta_b); + double theta_f_ = asin((f_x - x_link2) / link3); + if (is_front_ecd_obtuse) { + theta_f = std::numbers::pi - theta_f_; + } else { + theta_f = theta_f_; + } + return {theta_f, theta_b}; + } + void yaw_control() { + if (is_yaw_imu_control) { + *chassis_big_yaw_target_angle_error = + normalizeAngle(yaw_control_theta - *yaw_imu_angle); + } else { + std::array result = yaw_trajectory_controller.trajectory(); + yaw_control_theta = result[0]; + *chassis_big_yaw_target_angle_error = + normalizeAngle(yaw_control_theta - *chassis_big_yaw_angle); + } + } + void steering_control(const Eigen::Vector2d& move, double spin_speed) { + + Eigen::Vector2d lf_vel = Eigen::Vector2d{-spin_speed, spin_speed} + move; + Eigen::Vector2d lb_vel = Eigen::Vector2d{-spin_speed, -spin_speed} + move; + Eigen::Vector2d rb_vel = Eigen::Vector2d{spin_speed, -spin_speed} + move; + Eigen::Vector2d rf_vel = Eigen::Vector2d{spin_speed, spin_speed} + move; + + Eigen::Vector2d lb_vel_angle = lb_vel; + Eigen::Vector2d lf_vel_angle = lf_vel; + Eigen::Vector2d rb_vel_angle = rb_vel; + Eigen::Vector2d rf_vel_angle = rf_vel; + + double err[4] = { + -atan2(lb_vel_angle.y(), lb_vel_angle.x()) - *steering_lb_angle, + -atan2(lf_vel_angle.y(), lf_vel_angle.x()) - *steering_lf_angle, + -atan2(rb_vel_angle.y(), rb_vel_angle.x()) - *steering_rb_angle, + -atan2(rf_vel_angle.y(), rf_vel_angle.x()) - *steering_rf_angle}; + + *steering_lf_target_angle_error = norm_error_angle(err[1]); + *steering_lb_target_angle_error = norm_error_angle(err[0]); + *steering_rb_target_angle_error = norm_error_angle(err[2]); + *steering_rf_target_angle_error = norm_error_angle(err[3]); + *steering_wheel_lf_target_vel = + lf_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[1]); + *steering_wheel_lb_target_vel = + lb_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[0]); + *steering_wheel_rb_target_vel = + rb_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[2]); + *steering_wheel_rf_target_vel = + rf_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[3]); + } + void omniwheel_control(const Eigen::Vector2d& move) { + *omni_l_target_vel = move.x() * (speed_limit / wheel_r); + *omni_r_target_vel = move.x() * (speed_limit / wheel_r); + } + + void reset_motor() { + *steering_lf_target_angle_error = NAN; + *steering_lb_target_angle_error = NAN; + *steering_rb_target_angle_error = NAN; + *steering_rf_target_angle_error = NAN; + *steering_lf_target_angle_error = NAN; + *steering_lb_target_angle_error = NAN; + *steering_rb_target_angle_error = NAN; + *steering_rf_target_angle_error = NAN; + *omni_l_target_vel = NAN; + *omni_r_target_vel = NAN; + *steering_wheel_lf_target_vel = NAN; + *steering_wheel_lb_target_vel = NAN; + *steering_wheel_rb_target_vel = NAN; + *steering_wheel_rf_target_vel = NAN; + leg_lf_target_theta = NAN; + *leg_lb_target_theta = NAN; + *leg_rb_target_theta = NAN; + leg_rf_target_theta = NAN; + *leg_joint_lf_control_torque = NAN; + *leg_joint_rf_control_torque = NAN; + *chassis_big_yaw_target_angle_error = NAN; + } + static inline double norm_error_angle(const double& angle) { + double tmp = angle; + while (tmp > 2 * M_PI) + tmp -= 2 * M_PI; + while (tmp <= 0) + tmp += 2 * M_PI; + if (tmp > 2 * M_PI - tmp) + tmp = tmp - 2 * M_PI; + if (tmp > M_PI / 2) + tmp = tmp - M_PI; + else if (tmp < -M_PI / 2) + tmp = tmp + M_PI; + return tmp; + } + static inline double check_error_angle(const double& angle) { + double tmp = angle; + while (tmp > 2 * M_PI) + tmp -= 2 * M_PI; + while (tmp <= 0) + tmp += 2 * M_PI; + tmp -= M_PI; + + return sin(abs(tmp) - M_PI_2); + } + static double normalizeAngle(double angle) { + + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + + InputInterface arm_mode; + rmcs_msgs::ArmMode last_arm_mode; + pid::PidCalculator following_velocity_controller_; + + pid::PidCalculator leg_lf_error_pid_controller; + pid::PidCalculator leg_lf_velocity_pid_controller; + pid::PidCalculator leg_rf_error_pid_controller; + pid::PidCalculator leg_rf_velocity_pid_controller; + + double speed_limit = 4.5; // m/s + static constexpr double wheel_r = 0.11; // m + + const Eigen::Vector2d lf_vel_{1, -1}; + const Eigen::Vector2d lb_vel_{1, 1}; + const Eigen::Vector2d rb_vel_{-1, -1}; + const Eigen::Vector2d rf_vel_{-1, 1}; + Eigen::Vector2d lf_vel_last_{1, -1}; + Eigen::Vector2d lb_vel_last_{1, 1}; + Eigen::Vector2d rb_vel_last_{-1, -1}; + Eigen::Vector2d rf_vel_last_{-1, 1}; + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + + OutputInterface is_chassis_and_leg_enable; + // ————————————————————————steering—————————————————————————— + InputInterface steering_lf_angle; + InputInterface steering_lb_angle; + InputInterface steering_rb_angle; + InputInterface steering_rf_angle; + + OutputInterface steering_lf_target_angle_error; + OutputInterface steering_lb_target_angle_error; + OutputInterface steering_rb_target_angle_error; + OutputInterface steering_rf_target_angle_error; + OutputInterface steering_wheel_lf_target_vel; + OutputInterface steering_wheel_lb_target_vel; + OutputInterface steering_wheel_rb_target_vel; + OutputInterface steering_wheel_rf_target_vel; + // —————————————————————————leg———————————————————————————————— + OutputInterface omni_l_target_vel; + OutputInterface omni_r_target_vel; + InputInterface theta_lf; + InputInterface theta_lb; + InputInterface theta_rb; + InputInterface theta_rf; + + bool is_yaw_imu_control = false; + bool last_is_yaw_imu_control = false; + InputInterface yaw_imu_velocity; + InputInterface yaw_imu_angle; + InputInterface joint1_theta; + double yaw_control_theta = NAN; + double yaw_set_theta_in_YawFreeMode = NAN; + hardware::device::Trajectory yaw_trajectory_controller; + + bool is_leg_forward_joint_torque_control = false; + double leg_lf_target_theta = NAN; + OutputInterface leg_lb_target_theta; + OutputInterface leg_rb_target_theta; + double leg_rf_target_theta = NAN; + bool up_stairs_is_leg_press = false; + bool up_stairs_is_leg_lift = false; + + InputInterface leg_joint_lf_velocity; + InputInterface leg_joint_rf_velocity; + InputInterface leg_joint_lf_torque; + InputInterface leg_joint_rf_torque; + OutputInterface leg_joint_lf_control_torque; + OutputInterface leg_joint_rf_control_torque; + + OutputInterface chassis_big_yaw_target_angle_error; + InputInterface chassis_big_yaw_angle; + rmcs_msgs::ChassisMode chassis_mode = rmcs_msgs::ChassisMode::None; + rmcs_msgs::ChassisMode last_chassis_mode = rmcs_msgs::ChassisMode::None; + rmcs_msgs::LegMode leg_mode = rmcs_msgs::LegMode::Six_Wheel; + rmcs_msgs::LegMode last_leg_mode = rmcs_msgs::LegMode::None; + + hardware::device::Trajectory four_wheel_trajectory; + hardware::device::Trajectory six_wheel_trajectory; + hardware::device::Trajectory up_stairs_initial; + hardware::device::Trajectory up_stairs_leg_press; + hardware::device::Trajectory up_stairs_leg_lift; +}; +} // namespace rmcs_core::controller::chassis +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::Chassis_and_LegController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp index ce285f51..4b5d76fd 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/omni_wheel_controller.cpp @@ -1,236 +1,236 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "controller/pid/pid_calculator.hpp" - -namespace rmcs_core::controller::chassis { - -class OmniWheelController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - OmniWheelController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , following_velocity_controller_(30.0, 0.01, 300) { - following_velocity_controller_.integral_max = 40; - following_velocity_controller_.integral_min = -40; - - register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); - register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); - - register_input("/chassis/control_mode", mode_); - register_input("/chassis/control_move", move_); - register_input("/chassis/control_power_limit", power_limit_); - - register_output("/chassis/angle", chassis_angle_, nan); - register_output("/chassis/control_angle", chassis_control_angle_, nan); - - register_output( - "/chassis/left_front_wheel/control_velocity", left_front_control_velocity_, nan); - register_output( - "/chassis/left_back_wheel/control_velocity", left_back_control_velocity_, nan); - register_output( - "/chassis/right_back_wheel/control_velocity", right_back_control_velocity_, nan); - register_output( - "/chassis/right_front_wheel/control_velocity", right_front_control_velocity_, nan); - } - - void update() override { - auto move = move_->vector.head<2>().eval(); - if (std::isnan(move[0])) { - reset_all_controls(); - return; - } - - auto mode = *mode_; - if (last_mode_ != mode) { - if (mode == rmcs_msgs::ChassisMode::SPIN) - spinning_clockwise_ = !spinning_clockwise_; - else if (mode != rmcs_msgs::ChassisMode::AUTO) - following_velocity_controller_.reset(); - } - last_mode_ = mode; - - update_wheel_velocities(move); - } - - void reset_all_controls() { - last_mode_ = rmcs_msgs::ChassisMode::AUTO; - - *left_front_control_velocity_ = nan; - *left_back_control_velocity_ = nan; - *right_back_control_velocity_ = nan; - *right_front_control_velocity_ = nan; - } - - void update_wheel_velocities(Eigen::Vector2d move) { - double wheel_speed_limit = *power_limit_ >= 250 ? 150 : 80; - - if (move.norm() > 1) { - move.normalize(); - } - - double velocities[4]; - calculate_wheel_velocity_for_forwarding(wheel_speed_limit, velocities, move); - - double chassis_control_angle = nan; - switch (*mode_) { - case rmcs_msgs::ChassisMode::AUTO: break; - case rmcs_msgs::ChassisMode::SPIN: { - double wheel_speed = 0.4 * wheel_speed_limit; - add_wheel_velocity_for_spinning( - wheel_speed_limit, velocities, spinning_clockwise_ ? wheel_speed : -wheel_speed, - 0.1 * wheel_speed_limit); - } break; - case rmcs_msgs::ChassisMode::STEP_DOWN: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - // err: [0, 2pi) -> [0, alignment) -> signed. - // In step-down mode, two sides of the chassis can be used for alignment. - // TODO(qzh): Dynamically determine the split angle based on chassis velocity. - constexpr double alignment = std::numbers::pi; - while (err > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - err -= alignment; - } - - // Note: The chassis rotates in the opposite direction to the wheel motors. - double wheel_velocity = -std::clamp( - following_velocity_controller_.update(err), -wheel_speed_limit, wheel_speed_limit); - add_wheel_velocity_for_spinning( - wheel_speed_limit, velocities, wheel_velocity, 0.8 * wheel_speed_limit); - } break; - case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - // err: [0, 2pi) -> signed - // In launch ramp mode, only one direction can be used for alignment. - // TODO(qzh): Dynamically determine the split angle based on chassis velocity. - constexpr double alignment = 2 * std::numbers::pi; - if (err > alignment / 2) - err -= alignment; - - // Note: The chassis rotates in the opposite direction to the wheel motors. - double wheel_velocity = -std::clamp( - following_velocity_controller_.update(err), -wheel_speed_limit, wheel_speed_limit); - add_wheel_velocity_for_spinning( - wheel_speed_limit, velocities, wheel_velocity, 0.6 * wheel_speed_limit); - } break; - } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; - *chassis_control_angle_ = chassis_control_angle; - - *left_front_control_velocity_ = velocities[0]; - *left_back_control_velocity_ = velocities[1]; - *right_back_control_velocity_ = velocities[2]; - *right_front_control_velocity_ = velocities[3]; - } - - double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { - chassis_control_angle = *gimbal_yaw_angle_error_; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - // chassis_control_angle: [0, 2pi). - - // err = setpoint - measurement - // ^ ^ - // |gimbal_yaw_angle_error |chassis_angle - // ^ - // |(2pi - gimbal_yaw_angle) - double err = chassis_control_angle + *gimbal_yaw_angle_; - if (err >= 2 * std::numbers::pi) - err -= 2 * std::numbers::pi; - // err: [0, 2pi). - - return err; - } - - static inline void calculate_wheel_velocity_for_forwarding( - double wheel_speed_limit, double (&velocities)[4], Eigen::Vector2d move) { - - double right_oblique = wheel_speed_limit * (-move.y() * cos_45 + move.x() * sin_45); - double left_oblique = wheel_speed_limit * (move.x() * cos_45 + move.y() * sin_45); - - velocities[0] = right_oblique; - velocities[1] = left_oblique; - velocities[2] = -right_oblique; - velocities[3] = -left_oblique; - }; - - constexpr static inline void add_wheel_velocity_for_spinning( - double wheel_speed_limit, double (&velocities)[4], double velocity_for_spinning, - double min_acceptable_absolute_velocity_for_spinning) { - - double k = 1; - if (velocity_for_spinning > 0) { - double max_velocity_for_forwarding = - *std::max_element(std::begin(velocities), std::end(velocities)); - - double velocity_remaining = wheel_speed_limit - max_velocity_for_forwarding; - if (velocity_for_spinning > velocity_remaining) { - if (velocity_remaining > min_acceptable_absolute_velocity_for_spinning) - velocity_for_spinning = velocity_remaining; - else - k = (wheel_speed_limit - velocity_for_spinning) / max_velocity_for_forwarding; - } - } else if (velocity_for_spinning < 0) { - double min_velocity_for_forwarding = - *std::min_element(std::begin(velocities), std::end(velocities)); - - double velocity_remaining = -wheel_speed_limit - min_velocity_for_forwarding; - if (velocity_for_spinning < velocity_remaining) { - if (velocity_remaining < -min_acceptable_absolute_velocity_for_spinning) - velocity_for_spinning = velocity_remaining; - else - k = (-wheel_speed_limit - velocity_for_spinning) / min_velocity_for_forwarding; - } - } - - for (auto& velocity : velocities) - velocity = k * velocity + velocity_for_spinning; - } - -private: - static constexpr double inf = std::numeric_limits::infinity(); - static constexpr double nan = std::numeric_limits::quiet_NaN(); - - // Since sine and cosine function are not constexpr, we calculate once and cache them. - static inline const double sin_45 = std::sin(std::numbers::pi / 4.0); - static inline const double cos_45 = std::cos(std::numbers::pi / 4.0); - - InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; - - InputInterface mode_; - rmcs_msgs::ChassisMode last_mode_; - bool spinning_clockwise_ = false; - pid::PidCalculator following_velocity_controller_; - - InputInterface move_; - InputInterface power_limit_; - - OutputInterface chassis_angle_, chassis_control_angle_; - - OutputInterface left_front_control_velocity_; - OutputInterface left_back_control_velocity_; - OutputInterface right_back_control_velocity_; - OutputInterface right_front_control_velocity_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::OmniWheelController, rmcs_executor::Component) \ No newline at end of file +// #include +// #include +// #include +// #include + +// #include +// #include +// #include +// #include +// #include + +// #include "controller/pid/pid_calculator.hpp" + +// namespace rmcs_core::controller::chassis { + +// class OmniWheelController +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// OmniWheelController() +// : Node( +// get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) +// , following_velocity_controller_(30.0, 0.01, 300) { +// following_velocity_controller_.integral_max = 40; +// following_velocity_controller_.integral_min = -40; + +// register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); +// register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_); + +// register_input("/chassis/control_mode", mode_); +// register_input("/chassis/control_move", move_); +// register_input("/chassis/control_power_limit", power_limit_); + +// register_output("/chassis/angle", chassis_angle_, nan); +// register_output("/chassis/control_angle", chassis_control_angle_, nan); + +// register_output( +// "/chassis/left_front_wheel/control_velocity", left_front_control_velocity_, nan); +// register_output( +// "/chassis/left_back_wheel/control_velocity", left_back_control_velocity_, nan); +// register_output( +// "/chassis/right_back_wheel/control_velocity", right_back_control_velocity_, nan); +// register_output( +// "/chassis/right_front_wheel/control_velocity", right_front_control_velocity_, nan); +// } + +// void update() override { +// auto move = move_->vector.head<2>().eval(); +// if (std::isnan(move[0])) { +// reset_all_controls(); +// return; +// } + +// auto mode = *mode_; +// if (last_mode_ != mode) { +// if (mode == rmcs_msgs::ChassisMode::SPIN) +// spinning_clockwise_ = !spinning_clockwise_; +// else if (mode != rmcs_msgs::ChassisMode::AUTO) +// following_velocity_controller_.reset(); +// } +// last_mode_ = mode; + +// update_wheel_velocities(move); +// } + +// void reset_all_controls() { +// last_mode_ = rmcs_msgs::ChassisMode::AUTO; + +// *left_front_control_velocity_ = nan; +// *left_back_control_velocity_ = nan; +// *right_back_control_velocity_ = nan; +// *right_front_control_velocity_ = nan; +// } + +// void update_wheel_velocities(Eigen::Vector2d move) { +// double wheel_speed_limit = *power_limit_ >= 250 ? 150 : 80; + +// if (move.norm() > 1) { +// move.normalize(); +// } + +// double velocities[4]; +// calculate_wheel_velocity_for_forwarding(wheel_speed_limit, velocities, move); + +// double chassis_control_angle = nan; +// switch (*mode_) { +// case rmcs_msgs::ChassisMode::AUTO: break; +// case rmcs_msgs::ChassisMode::SPIN: { +// double wheel_speed = 0.4 * wheel_speed_limit; +// add_wheel_velocity_for_spinning( +// wheel_speed_limit, velocities, spinning_clockwise_ ? wheel_speed : -wheel_speed, +// 0.1 * wheel_speed_limit); +// } break; +// case rmcs_msgs::ChassisMode::STEP_DOWN: { +// double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + +// // err: [0, 2pi) -> [0, alignment) -> signed. +// // In step-down mode, two sides of the chassis can be used for alignment. +// // TODO(qzh): Dynamically determine the split angle based on chassis velocity. +// constexpr double alignment = std::numbers::pi; +// while (err > alignment / 2) { +// chassis_control_angle -= alignment; +// if (chassis_control_angle < 0) +// chassis_control_angle += 2 * std::numbers::pi; +// err -= alignment; +// } + +// // Note: The chassis rotates in the opposite direction to the wheel motors. +// double wheel_velocity = -std::clamp( +// following_velocity_controller_.update(err), -wheel_speed_limit, wheel_speed_limit); +// add_wheel_velocity_for_spinning( +// wheel_speed_limit, velocities, wheel_velocity, 0.8 * wheel_speed_limit); +// } break; +// case rmcs_msgs::ChassisMode::LAUNCH_RAMP: { +// double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + +// // err: [0, 2pi) -> signed +// // In launch ramp mode, only one direction can be used for alignment. +// // TODO(qzh): Dynamically determine the split angle based on chassis velocity. +// constexpr double alignment = 2 * std::numbers::pi; +// if (err > alignment / 2) +// err -= alignment; + +// // Note: The chassis rotates in the opposite direction to the wheel motors. +// double wheel_velocity = -std::clamp( +// following_velocity_controller_.update(err), -wheel_speed_limit, wheel_speed_limit); +// add_wheel_velocity_for_spinning( +// wheel_speed_limit, velocities, wheel_velocity, 0.6 * wheel_speed_limit); +// } break; +// } +// *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; +// *chassis_control_angle_ = chassis_control_angle; + +// *left_front_control_velocity_ = velocities[0]; +// *left_back_control_velocity_ = velocities[1]; +// *right_back_control_velocity_ = velocities[2]; +// *right_front_control_velocity_ = velocities[3]; +// } + +// double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { +// chassis_control_angle = *gimbal_yaw_angle_error_; +// if (chassis_control_angle < 0) +// chassis_control_angle += 2 * std::numbers::pi; +// // chassis_control_angle: [0, 2pi). + +// // err = setpoint - measurement +// // ^ ^ +// // |gimbal_yaw_angle_error |chassis_angle +// // ^ +// // |(2pi - gimbal_yaw_angle) +// double err = chassis_control_angle + *gimbal_yaw_angle_; +// if (err >= 2 * std::numbers::pi) +// err -= 2 * std::numbers::pi; +// // err: [0, 2pi). + +// return err; +// } + +// static inline void calculate_wheel_velocity_for_forwarding( +// double wheel_speed_limit, double (&velocities)[4], Eigen::Vector2d move) { + +// double right_oblique = wheel_speed_limit * (-move.y() * cos_45 + move.x() * sin_45); +// double left_oblique = wheel_speed_limit * (move.x() * cos_45 + move.y() * sin_45); + +// velocities[0] = right_oblique; +// velocities[1] = left_oblique; +// velocities[2] = -right_oblique; +// velocities[3] = -left_oblique; +// }; + +// constexpr static inline void add_wheel_velocity_for_spinning( +// double wheel_speed_limit, double (&velocities)[4], double velocity_for_spinning, +// double min_acceptable_absolute_velocity_for_spinning) { + +// double k = 1; +// if (velocity_for_spinning > 0) { +// double max_velocity_for_forwarding = +// *std::max_element(std::begin(velocities), std::end(velocities)); + +// double velocity_remaining = wheel_speed_limit - max_velocity_for_forwarding; +// if (velocity_for_spinning > velocity_remaining) { +// if (velocity_remaining > min_acceptable_absolute_velocity_for_spinning) +// velocity_for_spinning = velocity_remaining; +// else +// k = (wheel_speed_limit - velocity_for_spinning) / max_velocity_for_forwarding; +// } +// } else if (velocity_for_spinning < 0) { +// double min_velocity_for_forwarding = +// *std::min_element(std::begin(velocities), std::end(velocities)); + +// double velocity_remaining = -wheel_speed_limit - min_velocity_for_forwarding; +// if (velocity_for_spinning < velocity_remaining) { +// if (velocity_remaining < -min_acceptable_absolute_velocity_for_spinning) +// velocity_for_spinning = velocity_remaining; +// else +// k = (-wheel_speed_limit - velocity_for_spinning) / min_velocity_for_forwarding; +// } +// } + +// for (auto& velocity : velocities) +// velocity = k * velocity + velocity_for_spinning; +// } + +// private: +// static constexpr double inf = std::numeric_limits::infinity(); +// static constexpr double nan = std::numeric_limits::quiet_NaN(); + +// // Since sine and cosine function are not constexpr, we calculate once and cache them. +// static inline const double sin_45 = std::sin(std::numbers::pi / 4.0); +// static inline const double cos_45 = std::cos(std::numbers::pi / 4.0); + +// InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + +// InputInterface mode_; +// rmcs_msgs::ChassisMode last_mode_; +// bool spinning_clockwise_ = false; +// pid::PidCalculator following_velocity_controller_; + +// InputInterface move_; +// InputInterface power_limit_; + +// OutputInterface chassis_angle_, chassis_control_angle_; + +// OutputInterface left_front_control_velocity_; +// OutputInterface left_back_control_velocity_; +// OutputInterface right_back_control_velocity_; +// OutputInterface right_front_control_velocity_; +// }; + +// } // namespace rmcs_core::controller::chassis + +// #include + +// PLUGINLIB_EXPORT_CLASS( +// rmcs_core::controller::chassis::OmniWheelController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp new file mode 100644 index 00000000..2c8c8fa3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/qcp_solver.hpp @@ -0,0 +1,279 @@ +#pragma once + +#include +#include +#include + +#include + +namespace rmcs_core::controller::chassis { + +class QcpSolver { +public: + QcpSolver() = default; + + struct BoundaryConstraint { + double x_max; + double y_max; + }; + + struct RhombusConstraint { + double right, top; + }; + + struct QuadraticConstraint { + double a, b, c, d, e, f; + }; + + struct QuadraticConstraintMatrix { + Eigen::Matrix2d q; + Eigen::Vector2d p; + double r; + }; + + static Eigen::Vector2d solve( + const Eigen::Vector2d& objective, const BoundaryConstraint& boundary_constraint, + const RhombusConstraint& rhombus_constraint, + const QuadraticConstraint& quadratic_constraint) { + + auto linear_constraint = calculate_polygon_constraints( + rhombus_constraint.right, rhombus_constraint.top, boundary_constraint.x_max, + boundary_constraint.y_max); + + auto quadratic_constraint_matrixes = + calculate_quadratic_constraint_matrixes(quadratic_constraint); + auto quadratic_constraint_best_point = + calculate_quadratic_constraint_best_point(objective, quadratic_constraint_matrixes); + + if (is_point_inside_polygon(quadratic_constraint_best_point, linear_constraint)) + return quadratic_constraint_best_point; + else + return calculate_best_value_at_intersections( + objective, linear_constraint, quadratic_constraint_matrixes); + } + +private: + /** + * Calculates the feasible region of a linear program as a polygon. Returns a non-repeating list + * of vertices in counter-clockwise order representing the polygon. The function assumes that + * `rhombus_right` and `rhombus_top` are always positive values. + */ + static std::vector calculate_polygon_constraints( + double rhombus_right, double rhombus_top, double x_max, double y_max) { + std::vector polygon; + + if (y_max >= rhombus_top) { + polygon.emplace_back(rhombus_right, 0.0); + polygon.emplace_back(0.0, rhombus_top); + polygon.emplace_back(-rhombus_right, 0.0); + polygon.emplace_back(0.0, -rhombus_top); + } else if (y_max > 0) { + double intersecting_x = (rhombus_right / rhombus_top) * (rhombus_top - y_max); + polygon.emplace_back(rhombus_right, 0.0); + polygon.emplace_back(intersecting_x, y_max); + polygon.emplace_back(-intersecting_x, y_max); + polygon.emplace_back(-rhombus_right, 0.0); + polygon.emplace_back(0.0, -rhombus_top); + } else if (y_max > -rhombus_top) { + double intersecting_x = -(rhombus_right / rhombus_top) * (-rhombus_top - y_max); + polygon.emplace_back(intersecting_x, y_max); + polygon.emplace_back(-intersecting_x, y_max); + polygon.emplace_back(0.0, -rhombus_top); + } else if (y_max == -rhombus_top) { + polygon.emplace_back(0.0, -rhombus_top); + } + + sutherland_hodgman(polygon, {1, 0, -x_max}); + + return polygon; + } + + static void + sutherland_hodgman(std::vector& polygon, const Eigen::Vector3d& line) { + sutherland_hodgman( + polygon, + [&line](const Eigen::Vector2d& point) { return line.head<2>().dot(point) + line.z(); }, + [&line](const Eigen::Vector2d& p1, const Eigen::Vector2d& p2) -> Eigen::Vector2d { + Eigen::Vector3d line2 = + Eigen::Vector3d(p1.x(), p1.y(), 1).cross(Eigen::Vector3d(p2.x(), p2.y(), 1)); + Eigen::Matrix2d matrix{ + { line.x(), line.y()}, + {line2.x(), line2.y()} + }; + return matrix.inverse() * Eigen::Vector2d(-line.z(), -line2.z()); + }); + } + + /** + * Sutherland-Hodgman polygon clipping algorithm. The function modifies the input polygon by + * removing vertices that are outside the clipping region defined by the edge_compare and + * calculate_intersecting_point functions. + */ + static void sutherland_hodgman( + std::vector& polygon, const auto& edge_compare, + const auto& calculate_intersecting_point) { + + std::vector new_polygon; + for (size_t i = polygon.size() - 1, j = 0; j < polygon.size(); i = j++) { + auto& current_point = polygon[j]; + auto& prev_point = polygon[i]; + + if (edge_compare(current_point) == 0) { + new_polygon.emplace_back(current_point); + } else if (edge_compare(current_point) < 0) { + if (edge_compare(prev_point) > 0) + new_polygon.emplace_back( + calculate_intersecting_point(current_point, prev_point)); + new_polygon.emplace_back(current_point); + } else if (edge_compare(prev_point) < 0) { + if (polygon.size() != 2) [[likely]] // Prevent point duplicates when 2 vertices + new_polygon.emplace_back( + calculate_intersecting_point(current_point, prev_point)); + } + } + polygon.swap(new_polygon); + } + + static QuadraticConstraintMatrix + calculate_quadratic_constraint_matrixes(const QuadraticConstraint& quadratic_constraint) { + QuadraticConstraintMatrix result{ + // clang-format off + Eigen::Matrix2d{ + {2 * quadratic_constraint.a, quadratic_constraint.b}, + {quadratic_constraint.b, 2 * quadratic_constraint.c} + }, + Eigen::Vector2d{ + quadratic_constraint.d, quadratic_constraint.e + }, + quadratic_constraint.f + // clang-format on + }; + auto& [q, p, r] = result; + if (q(0, 0) < 0) + q = -q, p = -p, r = -r; + return result; + } + + static Eigen::Vector2d calculate_quadratic_constraint_best_point( + const Eigen::Vector2d& objective, const QuadraticConstraintMatrix& constraint) { + const auto& [q, p, r] = constraint; + + double det = q.determinant(); + Eigen::Matrix2d q_inv; + if (det < 0) + std::terminate(); + else if (det < epsilon_) + q_inv = (q + epsilon_ * Eigen::Matrix2d::Identity()).inverse(); + else + q_inv = q.inverse(); + auto q_inv_view = q_inv.selfadjointView(); + double lambda_inv = + std::sqrt((p.dot(q_inv_view * p) - 2 * r) / objective.dot(q_inv_view * objective)); + + return q_inv_view * (lambda_inv * objective - p); + } + + static bool is_point_inside_polygon( + const Eigen::Vector2d& p, const std::vector& polygon) { + // https://stackoverflow.com/a/63436180 + + // Helper function to check if a point is between two values + auto between = [](double p, double a, double b) { + return (p >= a && p <= b) || (p <= a && p >= b); + }; + + bool inside = false; + for (size_t i = polygon.size() - 1, j = 0; j < polygon.size(); i = j++) { + const Eigen::Vector2d& a = polygon[i]; + const Eigen::Vector2d& b = polygon[j]; + + // Corner cases + if ((p.x() == a.x() && p.y() == a.y()) || (p.x() == b.x() && p.y() == b.y())) + return true; // On edge + if (a.y() == b.y() && p.y() == a.y() && between(p.x(), a.x(), b.x())) + return true; // On edge + + if (between(p.y(), a.y(), b.y())) { // Check if P insides the vertical range + // Filter out "ray pass vertex" problem by treating the line a little lower + if ((p.y() == a.y() && b.y() >= a.y()) || (p.y() == b.y() && a.y() >= b.y())) + continue; + + // Calculate the cross product `PA X PB`, P lays on left side of AB if c > 0 + double c = (a.x() - p.x()) * (b.y() - p.y()) - (b.x() - p.x()) * (a.y() - p.y()); + if (c == 0) + return true; // On edge + + if ((a.y() < b.y()) == (c > 0)) + inside = !inside; + } + } + + return inside; + } + + static bool is_point_inside_quadratic_constraint( + const Eigen::Vector2d& point, const QuadraticConstraintMatrix& constraint) { + const auto& [q, p, r] = constraint; + return 0.5 * point.dot(q * point) + p.dot(point) + r <= 0; + } + + static Eigen::Vector2d calculate_best_value_at_intersections( + const Eigen::Vector2d& objective, const std::vector& linear_constraint, + const QuadraticConstraintMatrix& quadratic_constraint) { + const auto& [q, p, r] = quadratic_constraint; + + bool is_in_quadratic_constraint[20]; + if (sizeof(is_in_quadratic_constraint) < linear_constraint.size()) [[unlikely]] + std::terminate(); // TODO: Remove this + for (size_t i = 0; i < linear_constraint.size(); i++) + is_in_quadratic_constraint[i] = + is_point_inside_quadratic_constraint(linear_constraint[i], quadratic_constraint); + + double max_value = -inf_; + Eigen::Vector2d best_point = Eigen::Vector2d::Zero(); + + auto check_point = [&](Eigen::Vector2d point) { + double value = objective.dot(point); + if ((value > max_value) + || (value == max_value && point.x() >= best_point.x() + && point.y() >= best_point.y())) { + max_value = value; + best_point = point; + } + }; + + for (size_t i = linear_constraint.size() - 1, j = 0; j < linear_constraint.size(); + i = j++) { + if (is_in_quadratic_constraint[i]) + check_point(linear_constraint[i]); + if (!is_in_quadratic_constraint[i] || !is_in_quadratic_constraint[j]) { + const Eigen::Vector2d &x1 = linear_constraint[i], &x2 = linear_constraint[j]; + Eigen::Vector2d dx = x2 - x1; + + double a = 0.5 * dx.dot(q * dx); + double b = x1.dot(q * dx) + p.dot(dx); + double c = 0.5 * x1.dot(q * x1) + p.dot(x1) + r; + double delta = b * b - 4 * a * c; + + if (delta < 0) + continue; + + auto check_solution = [&](double t) { + if (0.0 < t && t < 1.0) + check_point(x1 + t * dx); + }; + check_solution((-b + sqrt(delta)) / (2 * a)); + check_solution((-b - sqrt(delta)) / (2 * a)); + } + } + + return best_point; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double epsilon_ = 1e-9; +}; + +} // namespace rmcs_core::controller::chassis \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp new file mode 100644 index 00000000..d1f782b8 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/steering_wheel_controller.cpp @@ -0,0 +1,504 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "controller/chassis/qcp_solver.hpp" +#include "controller/pid/matrix_pid_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "filter/low_pass_filter.hpp" + +namespace rmcs_core::controller::chassis { + +class SteeringWheelController + : public rmcs_executor::Component + , public rclcpp::Node { + + using Formula = std::tuple; + +public: + explicit SteeringWheelController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , mess_(get_parameter("mess").as_double()) + , moment_of_inertia_(get_parameter("moment_of_inertia").as_double()) + , vehicle_radius_(get_parameter("vehicle_radius").as_double()) + , wheel_radius_(get_parameter("wheel_radius").as_double()) + , friction_coefficient_(get_parameter("friction_coefficient").as_double()) + , k1_(get_parameter("k1").as_double()) + , k2_(get_parameter("k2").as_double()) + , no_load_power_(get_parameter("no_load_power").as_double()) + , control_acceleration_filter_(5.0, 1000.0) + , chassis_velocity_expected_(Eigen::Vector3d::Zero()) + , chassis_translational_velocity_pid_(5.0, 0.0, 1.0) + , chassis_angular_velocity_pid_(5.0, 0.0, 1.0) + , cos_varphi_(1, 0, -1, 0) // 0, pi/2, pi, 3pi/2 + , sin_varphi_(0, 1, 0, -1) + , steering_velocity_pid_(0.15, 0.0, 0.0) + , steering_angle_pid_(30.0, 0.0, 0.0) + , wheel_velocity_pid_(0.6, 0.0, 0.0) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + + register_input("/steering/steering/lf/angle", left_front_steering_angle_); + register_input("/steering/steering/lb/angle", left_back_steering_angle_); + register_input("/steering/steering/rb/angle", right_back_steering_angle_); + register_input("/steering/steering/rf/angle", right_front_steering_angle_); + + register_input("/steering/steering/lf/velocity", left_front_steering_velocity_); + register_input("/steering/steering/lb/velocity", left_back_steering_velocity_); + register_input("/steering/steering/rb/velocity", right_back_steering_velocity_); + register_input("/steering/steering/rf/velocity", right_front_steering_velocity_); + + register_input("/steering/wheel/lf/velocity", left_front_wheel_velocity_); + register_input("/steering/wheel/lb/velocity", left_back_wheel_velocity_); + register_input("/steering/wheel/rb/velocity", right_back_wheel_velocity_); + register_input("/steering/wheel/rf/velocity", right_front_wheel_velocity_); + + register_input("yaw_imu_velocity", chassis_yaw_velocity_imu_); + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_power_limit", power_limit_); + + register_output( + "/steering/steering/lf/control_torque", left_front_steering_control_torque_); + register_output( + "/steering/steering/lb/control_torque", left_back_steering_control_torque_); + register_output( + "/steering/steering/rb/control_torque", right_back_steering_control_torque_); + register_output( + "/steering/steering/rf/control_torque", right_front_steering_control_torque_); + + register_output( + "/steering/wheel/lf/control_torque", left_front_wheel_control_torque_); + register_output("/steering/wheel/lb/control_torque", left_back_wheel_control_torque_); + register_output( + "/steering/wheel/rb/control_torque", right_back_wheel_control_torque_); + register_output( + "/steering/wheel/rf/control_torque", right_front_wheel_control_torque_); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + integral_yaw_angle_imu(); + + auto steering_status = calculate_steering_status(); + auto wheel_velocities = calculate_wheel_velocities(); + auto chassis_velocity = calculate_chassis_velocity(steering_status, wheel_velocities); + + auto chassis_status_expected = calculate_chassis_status_expected(chassis_velocity); + auto chassis_control_velocity = calculate_chassis_control_velocity(); + + auto chassis_acceleration = calculate_chassis_control_acceleration( + chassis_status_expected.velocity, chassis_control_velocity); + + double power_limit = + *power_limit_ - no_load_power_ - k2_ * wheel_velocities.array().pow(2).sum(); + + auto wheel_pid_torques = + calculate_wheel_pid_torques(steering_status, wheel_velocities, chassis_status_expected); + + auto constrained_chassis_acceleration = constrain_chassis_control_acceleration( + steering_status, wheel_velocities, chassis_acceleration, wheel_pid_torques, + power_limit); + auto filtered_chassis_acceleration = + odom_to_base_link_vector(control_acceleration_filter_.update( + base_link_to_odom_vector(constrained_chassis_acceleration))); + + auto steering_torques = calculate_steering_control_torques( + steering_status, chassis_status_expected, filtered_chassis_acceleration); + auto wheel_torques = calculate_wheel_control_torques( + steering_status, filtered_chassis_acceleration, wheel_pid_torques); + + update_control_torques(steering_torques, wheel_torques); + update_chassis_velocity_expected(filtered_chassis_acceleration); + } + +private: + struct SteeringStatus { + Eigen::Vector4d angle, cos_angle, sin_angle; + Eigen::Vector4d velocity; + }; + + struct ChassisStatus { + Eigen::Vector3d velocity; + Eigen::Vector4d wheel_velocity_x, wheel_velocity_y; + }; + + void reset_all_controls() { + control_acceleration_filter_.reset(); + + chassis_yaw_angle_imu_ = 0.0; + chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + *left_front_steering_control_torque_ = 0.0; + *left_back_steering_control_torque_ = 0.0; + *right_back_steering_control_torque_ = 0.0; + *right_front_steering_control_torque_ = 0.0; + + *left_front_wheel_control_torque_ = 0.0; + *left_back_wheel_control_torque_ = 0.0; + *right_back_wheel_control_torque_ = 0.0; + *right_front_wheel_control_torque_ = 0.0; + } + + void integral_yaw_angle_imu() { + chassis_yaw_angle_imu_ += *chassis_yaw_velocity_imu_ * dt_; + chassis_yaw_angle_imu_ = std::fmod(chassis_yaw_angle_imu_, 2 * std::numbers::pi); + } + + SteeringStatus calculate_steering_status() { + SteeringStatus steering_status; + + steering_status.angle = { + *left_front_steering_angle_, // + *left_back_steering_angle_, // + *right_back_steering_angle_, // + *right_front_steering_angle_ // + }; + steering_status.angle.array() -= std::numbers::pi / 4; + steering_status.cos_angle = steering_status.angle.array().cos(); + steering_status.sin_angle = steering_status.angle.array().sin(); + + steering_status.velocity = { + *left_front_steering_velocity_, // + *left_back_steering_velocity_, // + *right_back_steering_velocity_, // + *right_front_steering_velocity_ // + }; + + return steering_status; + } + + Eigen::Vector4d calculate_wheel_velocities() { + return { + *left_front_wheel_velocity_, // + *left_back_wheel_velocity_, // + *right_back_wheel_velocity_, // + *right_front_wheel_velocity_ // + }; + } + + Eigen::Vector3d calculate_chassis_velocity( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities) const { + Eigen::Vector3d velocity; + double one_quarter_r = wheel_radius_ / 4.0; + velocity.x() = one_quarter_r * wheel_velocities.dot(steering_status.cos_angle); + velocity.y() = one_quarter_r * wheel_velocities.dot(steering_status.sin_angle); + velocity.z() = -one_quarter_r / vehicle_radius_ + * (-wheel_velocities[0] * steering_status.sin_angle[0] + + wheel_velocities[1] * steering_status.cos_angle[1] + + wheel_velocities[2] * steering_status.sin_angle[2] + - wheel_velocities[3] * steering_status.cos_angle[3]); + return velocity; + } + + ChassisStatus calculate_chassis_status_expected(const Eigen::Vector3d& chassis_velocity) { + auto calculate_energy = [this](const Eigen::Vector3d& velocity) { + return mess_ * velocity.head<2>().squaredNorm() + + moment_of_inertia_ * velocity.z() * velocity.z(); + }; + auto chassis_energy = calculate_energy(chassis_velocity); + auto chassis_energy_expected = calculate_energy(chassis_velocity_expected_); + if (chassis_energy_expected > chassis_energy) { + double k = std::sqrt(chassis_energy / chassis_energy_expected); + chassis_velocity_expected_ *= k; + } + + ChassisStatus chassis_status_expected; + chassis_status_expected.velocity = odom_to_base_link_vector(chassis_velocity_expected_); + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + chassis_status_expected.wheel_velocity_x = vx - vehicle_radius_ * vz * sin_varphi_.array(); + chassis_status_expected.wheel_velocity_y = vy + vehicle_radius_ * vz * cos_varphi_.array(); + + return chassis_status_expected; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity = chassis_control_velocity_->vector; + chassis_control_velocity.head<2>() = + Eigen::Rotation2Dd(-std::numbers::pi / 4) * chassis_control_velocity.head<2>(); + + return chassis_control_velocity; + } + + Eigen::Vector3d calculate_chassis_control_acceleration( + const Eigen::Vector3d& chassis_velocity_expected, + const Eigen::Vector3d& chassis_control_velocity) { + + Eigen::Vector2d translational_control_velocity = chassis_control_velocity.head<2>(); + Eigen::Vector2d translational_velocity = chassis_velocity_expected.head<2>(); + Eigen::Vector2d translational_control_acceleration = + chassis_translational_velocity_pid_.update( + translational_control_velocity - translational_velocity); + + const double& angular_control_velocity = chassis_control_velocity[2]; + const double& angular_velocity = chassis_velocity_expected[2]; + double angular_control_acceleration = + chassis_angular_velocity_pid_.update(angular_control_velocity - angular_velocity); + + Eigen::Vector3d chassis_control_acceleration; + chassis_control_acceleration << translational_control_acceleration, + angular_control_acceleration; + + if (chassis_control_acceleration.lpNorm<1>() < 1e-1) + chassis_control_acceleration.setZero(); + + return chassis_control_acceleration; + } + + Eigen::Vector4d calculate_wheel_pid_torques( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const ChassisStatus& chassis_status_expected) { + Eigen::Vector4d wheel_control_velocity = + chassis_status_expected.wheel_velocity_x.array() * steering_status.cos_angle.array() + + chassis_status_expected.wheel_velocity_y.array() * steering_status.sin_angle.array(); + return wheel_velocity_pid_.update( + wheel_control_velocity / wheel_radius_ - wheel_velocities); + } + + Eigen::Vector3d constrain_chassis_control_acceleration( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector3d& chassis_acceleration, const Eigen::Vector4d& wheel_pid_torques, + const double& power_limit) { + + Eigen::Vector2d translational_acceleration_direction = chassis_acceleration.head<2>(); + double translational_acceleration_max = translational_acceleration_direction.norm(); + if (translational_acceleration_max > 0.0) + translational_acceleration_direction /= translational_acceleration_max; + + double angular_acceleration_max = chassis_acceleration.z(); + double angular_acceleration_direction = angular_acceleration_max > 0 ? 1.0 : -1.0; + angular_acceleration_max *= angular_acceleration_direction; + + const double rhombus_right = friction_coefficient_ * g_; + const double rhombus_top = rhombus_right * mess_ * vehicle_radius_ / moment_of_inertia_; + + auto [a, b, c, d, e, f] = calculate_ellipse_parameters( + steering_status, wheel_velocities, translational_acceleration_direction, + angular_acceleration_direction, wheel_pid_torques); + + Eigen::Vector2d best_point = qcp_solver_.solve( + {1.0, 0.2}, {translational_acceleration_max, angular_acceleration_max}, + {rhombus_right, rhombus_top}, {a, b, c, d, e, f - power_limit}); + + Eigen::Vector3d best_acceleration; + best_acceleration << best_point.x() * translational_acceleration_direction, + best_point.y() * angular_acceleration_direction; + return best_acceleration; + } + + Eigen::Vector calculate_ellipse_parameters( + const SteeringStatus& steering_status, const Eigen::Vector4d& wheel_velocities, + const Eigen::Vector2d& translational_acceleration_direction, + const double& angular_acceleration_direction, const Eigen::Vector4d& wheel_torque_base) { + Eigen::Vector4d cos_alpha_minus_gamma = + steering_status.cos_angle.array() * translational_acceleration_direction.x() + + steering_status.sin_angle.array() * translational_acceleration_direction.y(); + Eigen::Vector4d sin_alpha_minus_varphi = + cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array(); + Eigen::Vector4d double_k1_torque_base_plus_wheel_velocities = + 2 * k1_ * wheel_torque_base.array() + wheel_velocities.array(); + + Eigen::Vector formula; + auto& [a, b, c, d, e, f] = formula; + + a = (k1_ * mess_ * mess_ * wheel_radius_ * wheel_radius_ / 16.0) + * cos_alpha_minus_gamma.array().pow(2).sum(); + b = ((k1_ * mess_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (8.0 * vehicle_radius_)) + * angular_acceleration_direction + * (cos_alpha_minus_gamma.array() * sin_alpha_minus_varphi.array()).sum(); + c = ((k1_ * moment_of_inertia_ * moment_of_inertia_ * wheel_radius_ * wheel_radius_) + / (16.0 * vehicle_radius_ * vehicle_radius_)) + * sin_alpha_minus_varphi.array().pow(2).sum(); + d = (mess_ * wheel_radius_ / 4.0) + * (double_k1_torque_base_plus_wheel_velocities.array() * cos_alpha_minus_gamma.array()) + .sum(); + e = ((moment_of_inertia_ * wheel_radius_) / (4.0 * vehicle_radius_)) + * angular_acceleration_direction + * (double_k1_torque_base_plus_wheel_velocities.array() * sin_alpha_minus_varphi.array()) + .sum(); + f = (wheel_torque_base.array() + * (k1_ * wheel_torque_base.array() + wheel_velocities.array())) + .sum(); + + return formula; + } + + Eigen::Vector4d calculate_steering_control_torques( + const SteeringStatus& steering_status, const ChassisStatus& chassis_status_expected, + const Eigen::Vector3d& chassis_acceleration) { + + const auto& [vx, vy, vz] = chassis_status_expected.velocity; + const auto& [ax, ay, az] = chassis_acceleration; + + Eigen::Vector4d dot_r_squared = chassis_status_expected.wheel_velocity_x.array().square() + + chassis_status_expected.wheel_velocity_y.array().square(); + + Eigen::Vector4d steering_control_velocities = + vx * ay - vy * ax - vz * (vx * vx + vy * vy) + + vehicle_radius_ * (az * vx - vz * (ax + vz * vy)) * cos_varphi_.array() + + vehicle_radius_ * (az * vy - vz * (ay - vz * vx)) * sin_varphi_.array(); + Eigen::Vector4d steering_control_angles; + + for (int i = 0; i < steering_control_velocities.size(); ++i) { + if (dot_r_squared[i] > 1e-2) { + steering_control_velocities[i] /= dot_r_squared[i]; + steering_control_angles[i] = std::atan2( + chassis_status_expected.wheel_velocity_y[i], + chassis_status_expected.wheel_velocity_x[i]); + } else { + auto x = ax - vehicle_radius_ * (az * sin_varphi_[i] + 0 * cos_varphi_[i]); + auto y = ay + vehicle_radius_ * (az * cos_varphi_[i] - 0 * sin_varphi_[i]); + if (x * x + y * y > 1e-6) { + steering_control_velocities[i] = 0.0; + steering_control_angles[i] = std::atan2(y, x); + } else { + steering_control_velocities[i] = nan_; + steering_control_angles[i] = nan_; + } + } + } + + Eigen::Vector4d steering_torques = steering_velocity_pid_.update( + steering_control_velocities + + steering_angle_pid_.update( + (steering_control_angles - steering_status.angle).unaryExpr([](double diff) { + diff = std::fmod(diff, std::numbers::pi); + if (diff < -std::numbers::pi / 2) { + diff += std::numbers::pi; + } else if (diff > std::numbers::pi / 2) { + diff -= std::numbers::pi; + } + return diff; + })) + - steering_status.velocity); + return steering_torques.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Eigen::Vector4d calculate_wheel_control_torques( + const SteeringStatus& steering_status, const Eigen::Vector3d& chassis_acceleration, + const Eigen::Vector4d& wheel_pid_torques) { + + const auto& [ax, ay, az] = chassis_acceleration; + Eigen::Vector4d wheel_torques = + wheel_radius_ + * (ax * mess_ * steering_status.cos_angle.array() + + ay * mess_ * steering_status.sin_angle.array() + + az * moment_of_inertia_ + * (cos_varphi_.array() * steering_status.sin_angle.array() + - sin_varphi_.array() * steering_status.cos_angle.array()) + / vehicle_radius_) + / 4.0; + + wheel_torques += wheel_pid_torques; + + return wheel_torques; + } + + void update_control_torques( + const Eigen::Vector4d& steering_torques, const Eigen::Vector4d& wheel_torques) { + *left_front_steering_control_torque_ = steering_torques[0]; + *left_back_steering_control_torque_ = steering_torques[1]; + *right_back_steering_control_torque_ = steering_torques[2]; + *right_front_steering_control_torque_ = steering_torques[3]; + + *left_front_wheel_control_torque_ = wheel_torques[0]; + *left_back_wheel_control_torque_ = wheel_torques[1]; + *right_back_wheel_control_torque_ = wheel_torques[2]; + *right_front_wheel_control_torque_ = wheel_torques[3]; + } + + void update_chassis_velocity_expected(const Eigen::Vector3d& chassis_acceleration) { + auto acceleration_odom = base_link_to_odom_vector(chassis_acceleration); + chassis_velocity_expected_ += dt_ * acceleration_odom; + } + + Eigen::Vector3d base_link_to_odom_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + Eigen::Vector3d odom_to_base_link_vector(Eigen::Vector3d vector) const { + vector.head<2>() = Eigen::Rotation2Dd(-chassis_yaw_angle_imu_) * vector.head<2>(); + return vector; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.81; + + const double mess_; + const double moment_of_inertia_; + const double vehicle_radius_; + const double wheel_radius_; + const double friction_coefficient_; + + const double k1_, k2_, no_load_power_; + + InputInterface joystick_right_; + InputInterface joystick_left_; + + InputInterface left_front_steering_angle_; + InputInterface left_back_steering_angle_; + InputInterface right_back_steering_angle_; + InputInterface right_front_steering_angle_; + + InputInterface left_front_steering_velocity_; + InputInterface left_back_steering_velocity_; + InputInterface right_back_steering_velocity_; + InputInterface right_front_steering_velocity_; + + InputInterface left_front_wheel_velocity_; + InputInterface left_back_wheel_velocity_; + InputInterface right_back_wheel_velocity_; + InputInterface right_front_wheel_velocity_; + + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + + OutputInterface left_front_steering_control_torque_; + OutputInterface left_back_steering_control_torque_; + OutputInterface right_back_steering_control_torque_; + OutputInterface right_front_steering_control_torque_; + + OutputInterface left_front_wheel_control_torque_; + OutputInterface left_back_wheel_control_torque_; + OutputInterface right_back_wheel_control_torque_; + OutputInterface right_front_wheel_control_torque_; + + QcpSolver qcp_solver_; + filter::LowPassFilter<3> control_acceleration_filter_; + + double chassis_yaw_angle_imu_ = 0.0; + Eigen::Vector3d chassis_velocity_expected_ = Eigen::Vector3d::Zero(); + + pid::MatrixPidCalculator<2> chassis_translational_velocity_pid_; + pid::PidCalculator chassis_angular_velocity_pid_; + + const Eigen::Vector4d cos_varphi_, sin_varphi_; + + pid::MatrixPidCalculator<4> steering_velocity_pid_, steering_angle_pid_, wheel_velocity_pid_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::SteeringWheelController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller.cpp new file mode 100644 index 00000000..2d253171 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller.cpp @@ -0,0 +1,362 @@ +// #include "controller/pid/pid_calculator.hpp" +// #include "hardware/device/trajectory.hpp" +// #include "rmcs_msgs/arm_mode.hpp" +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// namespace rmcs_core::controller::leg { +// class LegController +// : public rmcs_executor::Component +// , public rclcpp::Node { +// public: +// LegController() +// : Node( +// get_component_name(), +// rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) +// ,forward_x_position_in_FourWheel_(get_parameter("forward_x_position_in_FourWheel").as_double()){ + +// register_input("/remote/joystick/right", joystick_right_); +// register_input("/remote/joystick/left", joystick_left_); +// register_input("/remote/switch/right", switch_right_); +// register_input("/remote/switch/left", switch_left_); +// register_input("/remote/mouse/velocity", mouse_velocity_); // +// register_input("/remote/mouse", mouse_); // +// register_input("/remote/keyboard", keyboard_); + +// register_input("/move_speed_limit", speed_limit_); +// register_output("/leg/enable_flag", is_leg_enable, true); +// register_output("/leg/omni/l/target_vel", omni_l_target_vel, NAN); +// register_output("/leg/omni/r/target_vel", omni_r_target_vel, NAN); + +// register_input("/leg/encoder/lf/angle", theta_lf); +// register_input("/leg/encoder/lb/angle", theta_lb); +// register_input("/leg/encoder/rb/angle", theta_rb); +// register_input("/leg/encoder/rf/angle", theta_rf); + +// register_output("/leg/joint/lf/target_theta", leg_lf_target_theta, NAN); +// register_output("/leg/joint/rf/target_theta", leg_rf_target_theta, NAN); + +// register_output("/leg/joint/lb/target_theta", leg_lb_target_theta, NAN); +// register_output("/leg/joint/rb/target_theta", leg_rb_target_theta, NAN); + +// register_input("/arm/mode", arm_mode); + +// register_input("/arm/Joint1/theta", joint1_theta); +// register_input("/chassis/big_yaw/angle", chassis_big_yaw_angle); + + +// std::array four_wheel_angle = leg_inverse_kinematic(forward_x_position_in_FourWheel_,wheel_distance - forward_x_position_in_FourWheel_, false, false); +// four_wheel_trajectory +// .set_end_point( +// {four_wheel_angle[0], four_wheel_angle[1], four_wheel_angle[1], four_wheel_angle[0], +// 0, 0}) +// .set_total_step(500.0); +// std::array six_wheel_angle = leg_inverse_kinematic(250.0, 221.0, false, false); +// six_wheel_trajectory +// .set_end_point( +// {six_wheel_angle[0], six_wheel_angle[1], six_wheel_angle[1], six_wheel_angle[0], 0, +// 0}) +// .set_total_step(500.0); + +// up_stairs_initial.set_end_point({1.700270, 1.680, 1.680, 1.700270, 0, 0}) +// .set_total_step(2000); +// up_stairs_leg_press.set_end_point({0.506814, 0.995241, 0.995241, 0.466814, 0, 0}) +// .set_total_step(1000); +// up_stairs_leg_lift +// .set_end_point({1.2193598767, 1.261730456, 1.261730456, 1.2193598767, 0, 0}) +// .set_total_step(3100); +// } + +// void update() override { +// auto switch_right = *switch_right_; +// auto switch_left = *switch_left_; +// auto mouse = *mouse_; +// auto keyboard = *keyboard_; +// using namespace rmcs_msgs; +// if (!initial_check_done_) { +// *is_leg_enable = false; +// reset_motor(); +// leg_mode = rmcs_msgs::LegMode::None; +// if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + +// initial_check_done_ = true; +// } +// } else { +// if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) +// || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { +// *is_leg_enable = false; +// reset_motor(); +// leg_mode = rmcs_msgs::LegMode::None; +// } else { +// *is_leg_enable = true; +// Eigen::Rotation2D rotation(*chassis_big_yaw_angle + *joint1_theta); +// // Eigen::Vector2d move_ = rotation * (*joystick_left_); + +// Eigen::Vector2d move_ = (*joystick_left_); +// mode_selection(); +// if (*arm_mode == rmcs_msgs::ArmMode::Auto_Spin) { +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; +// } +// omniwheel_control(move_); +// leg_control(); + +// last_arm_mode = *arm_mode; +// last_leg_mode = leg_mode; +// } +// } +// } + +// private: +// void mode_selection() { +// auto switch_right = *switch_right_; +// auto switch_left = *switch_left_; +// auto mouse = *mouse_; +// auto keyboard = *keyboard_; +// using namespace rmcs_msgs; + +// if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) { + +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; + +// } else if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) { + +// leg_mode = rmcs_msgs::LegMode::Six_Wheel; + +// } else if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) { + +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; + +// } else if (switch_left == Switch::DOWN && switch_right == Switch::UP) { +// if (keyboard.v) { +// if (!keyboard.shift && !keyboard.ctrl) { +// leg_mode = rmcs_msgs::LegMode::Six_Wheel; +// } +// if (keyboard.shift && !keyboard.ctrl) { +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; +// } +// } +// if (keyboard.b) { +// leg_mode = rmcs_msgs::LegMode::Up_Stairs; +// up_stairs_is_leg_press = false; +// up_stairs_is_leg_lift = false; +// up_stairs_initial.reset(); +// up_stairs_leg_press.reset(); +// up_stairs_leg_lift.reset(); +// up_stairs_initial.set_start_point( +// {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); +// } +// if (keyboard.d) { +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; +// } + +// if (last_arm_mode != *arm_mode) { +// if (*arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Left +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Mid +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Gold_Right +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Sliver +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Ground +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_LB +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Storage_RB +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Extract +// || *arm_mode == rmcs_msgs::ArmMode::Customer +// || *arm_mode == rmcs_msgs::ArmMode::Auto_Up_Stairs) { + +// if (*arm_mode == rmcs_msgs::ArmMode::Customer) { +// leg_mode = rmcs_msgs::LegMode::Four_Wheel; +// } else { +// leg_mode = rmcs_msgs::LegMode::Six_Wheel; +// } +// } else if (*arm_mode == rmcs_msgs::ArmMode::Auto_Walk) { +// leg_mode = rmcs_msgs::LegMode::Six_Wheel; +// } +// } + +// } else { +// leg_mode = rmcs_msgs::LegMode::None; +// } +// } + +// void reset_motor() { +// *omni_l_target_vel = NAN; +// *omni_r_target_vel = NAN; +// *leg_lf_target_theta = NAN; +// *leg_lb_target_theta = NAN; +// *leg_rb_target_theta = NAN; +// *leg_rf_target_theta = NAN; +// } + +// void leg_control() { + +// auto reset_leg_trajectory = [&] { +// if (last_leg_mode != leg_mode) { +// if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { +// four_wheel_trajectory.reset(); +// four_wheel_trajectory.set_start_point( +// {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + +// } else if (leg_mode == rmcs_msgs::LegMode::Six_Wheel) { +// six_wheel_trajectory.reset(); +// six_wheel_trajectory.set_start_point( +// {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); +// } +// } +// }; +// reset_leg_trajectory(); + +// static std::array result; + +// if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { +// result = four_wheel_trajectory.trajectory(); + +// } else if (leg_mode == rmcs_msgs::LegMode::Six_Wheel) { +// if (!six_wheel_trajectory.get_complete()) { +// result = six_wheel_trajectory.trajectory(); +// } else { +// result = six_wheel_trajectory.trajectory(); +// if (*theta_lf < std::numbers::pi / 2.0 || *theta_rf < std::numbers::pi / 2.0) { +// result[0] = NAN; +// result[3] = NAN; +// } +// } +// } else if (leg_mode == rmcs_msgs::LegMode::Up_Stairs) { +// if (!up_stairs_initial.get_complete()) { +// result = up_stairs_initial.trajectory(); +// up_stairs_leg_press.set_start_point( +// {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); +// up_stairs_is_leg_press = false; +// } else { +// if (keyboard_->shift) { +// up_stairs_is_leg_press = true; +// } +// if (keyboard_->ctrl) { +// up_stairs_is_leg_lift = true; +// } else if (up_stairs_is_leg_press) { +// if (!up_stairs_leg_press.get_complete()) { +// result = up_stairs_leg_press.trajectory(); +// up_stairs_leg_lift.set_start_point( +// {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); +// } else if (up_stairs_is_leg_lift) { +// result = up_stairs_leg_lift.trajectory(); +// if (up_stairs_leg_lift.get_complete()) { +// result[0] = 0.8; +// result[3] = 0.8; +// } +// } +// } +// } +// } else if (leg_mode == rmcs_msgs::LegMode::None) { +// result[0] = *theta_lf; +// result[1] = *theta_lb; +// result[2] = *theta_rb; +// result[3] = *theta_rf; +// } +// leg_joint_controller(result[0], result[1], result[2], result[3]); +// } + +// void leg_joint_controller(double lf, double lb, double rb, double rf) { + +// *leg_lf_target_theta = lf; +// *leg_rf_target_theta = rf; +// *leg_lb_target_theta = lb; +// *leg_rb_target_theta = rb; +// }; + +// static std::array leg_inverse_kinematic( +// double f_x, double b_x, bool is_front_ecd_obtuse, bool is_back_ecd_obtuse) { +// constexpr double link1 = 240.0f, link2 = 120.0f, link3 = 160.0f; +// constexpr double link_angle = 5 * std::numbers::pi / 6.0; +// double theta_f, theta_b; +// theta_b = !is_back_ecd_obtuse ? asin(b_x / link1) : (M_PI - asin(b_x / link1)); +// double x_link2 = link2 * sin(link_angle - theta_b); +// theta_f = !is_front_ecd_obtuse ? asin((f_x - x_link2) / link3) +// : (M_PI - asin((f_x - x_link2) / link3)); + +// return {theta_f, theta_b}; +// } + +// void omniwheel_control(const Eigen::Vector2d& move) { +// if (leg_mode != rmcs_msgs::LegMode::Four_Wheel) { +// *omni_l_target_vel = move.x() * (*speed_limit_ / wheel_r); +// *omni_r_target_vel = move.x() * (*speed_limit_ / wheel_r); +// } +// else { +// *omni_l_target_vel = NAN; +// *omni_r_target_vel = NAN; + +// } +// } + +// static double normalizeAngle(double angle) { + +// while (angle > M_PI) +// angle -= 2 * M_PI; +// while (angle < -M_PI) +// angle += 2 * M_PI; +// return angle; +// } + + +// const double forward_x_position_in_FourWheel_; +// InputInterface arm_mode; +// InputInterface chassis_mode; +// rmcs_msgs::ArmMode last_arm_mode; + +// static constexpr double wheel_r = 0.11; +// InputInterface speed_limit_; // m/s +// InputInterface joystick_right_; +// InputInterface joystick_left_; +// InputInterface switch_right_; +// InputInterface switch_left_; +// InputInterface mouse_velocity_; +// InputInterface mouse_; +// InputInterface keyboard_; +// OutputInterface is_leg_enable; + +// OutputInterface omni_l_target_vel; +// OutputInterface omni_r_target_vel; +// InputInterface theta_lf; +// InputInterface theta_lb; +// InputInterface theta_rb; +// InputInterface theta_rf; + +// OutputInterface leg_lf_target_theta; +// OutputInterface leg_rf_target_theta; +// OutputInterface leg_lb_target_theta; +// OutputInterface leg_rb_target_theta; + +// bool up_stairs_is_leg_press = false; +// bool up_stairs_is_leg_lift = false; + +// rmcs_msgs::LegMode leg_mode = rmcs_msgs::LegMode::Six_Wheel; +// rmcs_msgs::LegMode last_leg_mode = rmcs_msgs::LegMode::None; + +// InputInterface chassis_big_yaw_angle; +// InputInterface joint1_theta; + +// hardware::device::Trajectory four_wheel_trajectory; +// static constexpr double wheel_distance = 458.0f; +// hardware::device::Trajectory six_wheel_trajectory; +// hardware::device::Trajectory up_stairs_initial; +// hardware::device::Trajectory up_stairs_leg_press; +// hardware::device::Trajectory up_stairs_leg_lift; +// bool initial_check_done_ = false; +// }; + +// } // namespace rmcs_core::controller::leg +// #include + +// PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::leg::LegController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller_dev.cpp b/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller_dev.cpp new file mode 100644 index 00000000..18359e77 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/leg/leg_controller_dev.cpp @@ -0,0 +1,484 @@ +#include "controller/pid/pid_calculator.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/hsm/HSM_up_one_stair_dev.hpp" +#include "hardware/hsm/HSM_up_stairs_dev.hpp" +#include "rmcs_msgs/arm_mode.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::leg { + +class LegController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + LegController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , forward_x_position_in_FourWheel_( + get_parameter("forward_x_position_in_FourWheel").as_double()) + , forward_x_position_in_SixWheel_( + get_parameter("forward_x_position_in_SixWheel").as_double()) + , backward_x_position_in_SixWheel_( + get_parameter("backward_x_position_in_SixWheel").as_double()) + , initial_end_point_(get_parameter("initial_end_point").as_double_array()) + , press_end_point_(get_parameter("press_end_point").as_double_array()) + , lift_end_point_(get_parameter("lift_end_point").as_double_array()) + , lift_and_initial_end_point_(get_parameter("lift_and_initial_end_point").as_double_array()) + , initial_again_end_point_(get_parameter("initial_again_end_point").as_double_array()) + , k_(get_parameter("k").as_double_array()) + , b_(get_parameter("b").as_double_array()) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); // + register_input("/remote/mouse", mouse_); // + register_input("/remote/keyboard", keyboard_); + + // register_input("/move_speed_limit", speed_limit_); + register_input("/chassis/control_velocity", chassis_velocity_); + register_output("/leg/enable_flag", is_leg_enable, false); + register_output("/leg/omni/l/target_vel", omni_l_target_vel, NAN); + register_output("/leg/omni/r/target_vel", omni_r_target_vel, NAN); + + register_input("/leg/encoder/lf/angle", theta_lf); + register_input("/leg/encoder/lb/angle", theta_lb); + register_input("/leg/encoder/rb/angle", theta_rb); + register_input("/leg/encoder/rf/angle", theta_rf); + + register_output("/leg/joint/lf/target_theta", leg_lf_target_theta, NAN); + register_output("/leg/joint/rf/target_theta", leg_rf_target_theta, NAN); + + register_output("/leg/joint/lb/target_theta", leg_lb_target_theta, NAN); + register_output("/leg/joint/rb/target_theta", leg_rb_target_theta, NAN); + + register_output("/leg/lf_result", lf_result_, 0); + register_output("/leg/lb_result", lb_result_, 0); + register_output("/leg/rf_result", rf_result_, 0); + register_output("/leg/rb_result", rb_result_, 0); + register_output("/leg/velocity_in_x", velocity_in_x, 0); + + register_input("/arm/mode", arm_mode); + + register_input("/arm/Joint1/theta", joint1_theta); + register_input("/chassis/big_yaw/angle", chassis_big_yaw_angle); + + // register_input("/tof/distance",tof_distance_); + + std::array four_wheel_angle = leg_inverse_kinematic( + forward_x_position_in_FourWheel_, wheel_distance - forward_x_position_in_FourWheel_, + false, false); + four_wheel_trajectory + .set_end_point( + {four_wheel_angle[0], four_wheel_angle[1], four_wheel_angle[1], four_wheel_angle[0], + 0, 0}) + .set_total_step(1000); + std::array six_wheel_angle = leg_inverse_kinematic( + forward_x_position_in_SixWheel_, backward_x_position_in_SixWheel_, false, false); + six_wheel_trajectory + .set_end_point( + {six_wheel_angle[0], six_wheel_angle[1], six_wheel_angle[1], six_wheel_angle[0], 0, + 0}) + .set_total_step(500); + + down_stairs_trajectory.set_end_point({1.151109, 1.694066, 1.694066, 1.151109, 0, 0}) + .set_total_step(800); + up_stairs + .init_and_trajectory_set( + initial_end_point_, press_end_point_, lift_end_point_, lift_and_initial_end_point_, + initial_again_end_point_) + .bind("theta_lf", theta_lf_ptr) + .bind("theta_lb", theta_lb_ptr) + .bind("theta_rb", theta_rb_ptr) + .bind("theta_rf", theta_rf_ptr) + .bind("speed", chassis_velocity_ptr) + .bind("result", result) + .bind("b",b_) + .bind("k",k_); + + // up_stairs_initial + // .set_end_point( + // {initial_end_point_[0], initial_end_point_[1], initial_end_point_[2], + // initial_end_point_[3], 0, 0}) + // .set_total_step(1500); + // up_stairs_leg_press.set_end_point({0.406814, 0.395241, 0.395241, 0.4066814, 0, 0}); + // up_stairs_leg_lift.set_end_point( + // {lift_end_point_[0], lift_end_point_[1], lift_end_point_[2], lift_end_point_[3], 0, + // 0}); + // up_stairs_leg_press_and_lift.set_end_point( + // {press_and_lift_end_point_[0], press_and_lift_end_point_[1], + // press_and_lift_end_point_[2], press_and_lift_end_point_[3], 0, 0}); + + // RCLCPP_INFO(this->get_logger(), " lf%f lb%f",six_wheel_angle[0], six_wheel_angle[1]); + } + + void update() override { + // RCLCPP_INFO(this->get_logger(), " lf%f lb%f", *theta_lf, *theta_lb); + *velocity_in_x = (*chassis_velocity_)->x(); + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + using namespace rmcs_msgs; + if (!initial_check_done_) { + *is_leg_enable = false; + reset_motor(); + leg_mode = rmcs_msgs::LegMode::None; + if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + + initial_check_done_ = true; + } + } else { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + *is_leg_enable = false; + reset_motor(); + leg_mode = rmcs_msgs::LegMode::None; + } else { + *is_leg_enable = true; + Eigen::Rotation2D rotation(*chassis_big_yaw_angle + *joint1_theta); + // Eigen::Vector2d move_ = rotation * (*joystick_left_); + // Eigen::Vector2d move_ = *joystick_left_; + mode_selection(); + if (*arm_mode == rmcs_msgs::ArmMode::Auto_Spin) { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + } + omniwheel_control(); + leg_control(); + // RCLCPP_INFO( + // this->get_logger(), "%f %f %f %f", result[0], result[1], result[2], result[3]); + + last_arm_mode = *arm_mode; + last_leg_mode = leg_mode; + // last_upstairs_mode = up_stairs_mode; + { + *lf_result_ = (result)[0]; + *lb_result_ = (result)[1]; + *rb_result_ = (result)[2]; + *rf_result_ = (result)[3]; + } + } + } + } + +private: + void mode_selection() { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto mouse = *mouse_; + auto keyboard = *keyboard_; + using namespace rmcs_msgs; + + if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) { + // RCLCPP_INFO(this->get_logger(),"switch -> four wheel"); + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::DOWN) { + // RCLCPP_INFO(this->get_logger(),"switch -> six wheel"); + // RCLCPP_INFO(this->get_logger(),"switch go to six "); + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + + } else if (switch_left == Switch::MIDDLE && switch_right == Switch::UP) { + + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + + } else if (switch_left == Switch::DOWN && switch_right == Switch::UP) { + if (keyboard.v) { + if (!keyboard.shift && !keyboard.ctrl) { + + // RCLCPP_INFO(this->get_logger(),"V -> six wheel"); + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + } + if (keyboard.shift && !keyboard.ctrl) { + // RCLCPP_INFO(this->get_logger(),"V -> four wheel"); + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + } + } + + if (keyboard.d) { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + // RCLCPP_INFO(this->get_logger(),"d -> four wheel"); + } + + // if (keyboard.q) { + // leg_mode = rmcs_msgs::LegMode::Down_Stairs; + // } + + if (last_arm_mode != *arm_mode) { + + switch (*arm_mode) { + case rmcs_msgs::ArmMode::Customer: { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + break; + }; + case rmcs_msgs::ArmMode::Auto_Spin: { + leg_mode = rmcs_msgs::LegMode::Four_Wheel; + break; + }; + case rmcs_msgs::ArmMode::Auto_Up_Stairs: { + RCLCPP_INFO(this->get_logger(),"arm to up"); + + + leg_mode = rmcs_msgs::LegMode::Up_Stairs; + up_stairs.stop(); + up_stairs.start(UpStairsState::Initial); + + break; + // mode换成上台阶 + } + case rmcs_msgs::ArmMode::Auto_Down_Stairs: { + leg_mode = rmcs_msgs::LegMode::Down_Stairs; // 同时按下 q和shift + break; + } + default: { + // RCLCPP_INFO(this->get_logger(),"arm go to six "); + leg_mode = rmcs_msgs::LegMode::Six_Wheel; + break; + } + } + } + } else { + leg_mode = rmcs_msgs::LegMode::None; + } + } + + void reset_motor() { + *omni_l_target_vel = NAN; + *omni_r_target_vel = NAN; + *leg_lf_target_theta = NAN; + *leg_lb_target_theta = NAN; + *leg_rb_target_theta = NAN; + *leg_rf_target_theta = NAN; + } + + void leg_control() { + + // RCLCPP_INFO(this->get_logger(), "leg_mode = %d", static_cast(leg_mode)); + if (last_leg_mode != leg_mode) { + + if (leg_mode == rmcs_msgs::LegMode::Four_Wheel) { + four_wheel_trajectory.reset(); + // RCLCPP_INFO(this->get_logger(), "change and reset"); + four_wheel_trajectory.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + if (last_leg_mode == rmcs_msgs::LegMode::Up_Stairs + || last_leg_mode == rmcs_msgs::LegMode::Down_Stairs) { + four_wheel_trajectory.set_total_step(1500); + } + } else if (leg_mode == rmcs_msgs::LegMode::Six_Wheel) { + six_wheel_trajectory.reset(); + six_wheel_trajectory.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + if (last_leg_mode == rmcs_msgs::LegMode::Up_Stairs + || last_leg_mode == rmcs_msgs::LegMode::Down_Stairs) { + six_wheel_trajectory.set_total_step(1500); + } + + } else if (leg_mode == rmcs_msgs::LegMode::Down_Stairs) { + down_stairs_trajectory.reset(); + down_stairs_trajectory.set_start_point( + {*theta_lf, *theta_lb, *theta_rb, *theta_rf, 0.0, 0.0}); + } + } + switch (leg_mode) { + case rmcs_msgs::LegMode::Four_Wheel: four_wheel_controller(); break; + case rmcs_msgs::LegMode::Six_Wheel: six_wheel_controller(); break; + case rmcs_msgs::LegMode::Up_Stairs: up_stairs_controller();break; + case rmcs_msgs::LegMode::Down_Stairs: down_stairs_controller(); break; + case rmcs_msgs::LegMode::None: { + (result)[0] = *theta_lf; + (result)[1] = *theta_lb; + (result)[2] = *theta_rb; + (result)[3] = *theta_rf; + break; + } + } + leg_joint_controller((result)[0], (result)[1], (result)[2], (result)[3]); + } + + void leg_joint_controller(double lf, double lb, double rb, double rf) { + + *leg_lf_target_theta = lf; + *leg_rf_target_theta = rf; + *leg_lb_target_theta = lb; + *leg_rb_target_theta = rb; + }; + void four_wheel_controller() { result = four_wheel_trajectory.trajectory(); } + + void six_wheel_controller() { + if (!six_wheel_trajectory.get_complete()) { + result = six_wheel_trajectory.trajectory(); + } else { + result = six_wheel_trajectory.trajectory(); + if (*theta_lf < std::numbers::pi / 2.0 || *theta_rf < std::numbers::pi / 2.0) { + (result)[0] = NAN; + (result)[3] = NAN; + } + } + } + + void down_stairs_controller() { result = down_stairs_trajectory.trajectory(); } + void up_stairs_controller() { + //(this->get_logger(),"go to up"); + if (keyboard_->shift&&!keyboard_->ctrl) { + //RCLCPP_INFO(this->get_logger(),"go to shift"); + up_stairs.processEvent( + rmcs_core::hardware::hsm::up_stairs_hsm::events::go_to_TwoProcess_lift); + } + + if (keyboard_->ctrl&&!keyboard_->shift) { + up_stairs.processEvent( + rmcs_core::hardware::hsm::up_stairs_hsm::events::go_to_OneProcess); + } + + if(keyboard_->ctrl&&keyboard_->shift){ + up_stairs.processEvent(rmcs_core::hardware::hsm::up_stairs_hsm::events::go_to_TwoProcess_initial); + } + + + up_stairs.processEvent(rmcs_core::hardware::hsm::up_stairs_hsm::events::tick); + + result = up_stairs.get_result(); + // RCLCPP_INFO(this->get_logger(), "当前期望角度 lf%f lb%f", result[0], result[1]); + } + + static std::array leg_inverse_kinematic( + double f_x, double b_x, bool is_front_ecd_obtuse, bool is_back_ecd_obtuse) { + constexpr double link1 = 240.0f, link2 = 120.0f, link3 = 160.0f; + constexpr double link_angle = 5 * std::numbers::pi / 6.0; + double theta_f, theta_b; + theta_b = !is_back_ecd_obtuse ? asin(b_x / link1) : (M_PI - asin(b_x / link1)); + double x_link2 = link2 * sin(link_angle - theta_b); + theta_f = !is_front_ecd_obtuse ? asin((f_x - x_link2) / link3) + : (M_PI - asin((f_x - x_link2) / link3)); + + return {theta_f, theta_b}; + } + + void omniwheel_control() { + if (leg_mode != rmcs_msgs::LegMode::Four_Wheel) { + *omni_l_target_vel = (*chassis_velocity_)->x() / wheel_r; + *omni_r_target_vel = (*chassis_velocity_)->x() / wheel_r; + } else { + *omni_l_target_vel = NAN; + *omni_r_target_vel = NAN; + } + } + + static double normalizeAngle(double angle) { + + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + + std::array result; + const double forward_x_position_in_FourWheel_; + const double forward_x_position_in_SixWheel_; + const double backward_x_position_in_SixWheel_; + const std::vector initial_end_point_; + const std::vector press_end_point_; + const std::vector lift_end_point_; + const std::vector lift_and_initial_end_point_; + const std::vector initial_again_end_point_; + std::vector k_; + std::vector b_; + + InputInterface arm_mode; + InputInterface chassis_mode; + rmcs_msgs::ArmMode last_arm_mode = rmcs_msgs::ArmMode::None; + + static constexpr double wheel_r = 0.11; + // static constexpr double v_reference = 1.5; + // InputInterface speed_limit_; // m/s + InputInterface chassis_velocity_; + InputInterface* chassis_velocity_ptr=&chassis_velocity_; + + + InputInterface tof_distance_; + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + OutputInterface is_leg_enable; + + OutputInterface velocity_in_x; + OutputInterface omni_l_target_vel; + OutputInterface omni_r_target_vel; + InputInterface theta_lf; + InputInterface theta_lb; + InputInterface theta_rb; + InputInterface theta_rf; + + + InputInterface* theta_lf_ptr=&theta_lf; + InputInterface* theta_lb_ptr=&theta_lb; + InputInterface* theta_rb_ptr=&theta_rb; + InputInterface* theta_rf_ptr=&theta_rf; + + OutputInterface leg_lf_target_theta; + OutputInterface leg_rf_target_theta; + OutputInterface leg_lb_target_theta; + OutputInterface leg_rb_target_theta; + + OutputInterface lf_result_; + OutputInterface lb_result_; + OutputInterface rf_result_; + OutputInterface rb_result_; + + // bool up_stairs_is_leg_press = false; + // bool up_stairs_is_leg_lift = false; + + rmcs_msgs::LegMode leg_mode = rmcs_msgs::LegMode::Six_Wheel; + rmcs_msgs::LegMode last_leg_mode = rmcs_msgs::LegMode::None; + // rmcs_msgs::UpStairsMode last_upstairs_mode = rmcs_msgs::UpStairsMode::Step_By_Two; + // rmcs_msgs::UpStairsMode up_stairs_mode = rmcs_msgs::UpStairsMode::Step_By_Two; + InputInterface chassis_big_yaw_angle; + InputInterface joint1_theta; + + hardware::device::Trajectory four_wheel_trajectory; + static constexpr double wheel_distance = 458.0f; + hardware::device::Trajectory six_wheel_trajectory; + + hardware::device::Trajectory down_stairs_trajectory; + + // hardware::device::Trajectory + // up_stairs_initial; + // hardware::device::Trajectory + // up_stairs_leg_press; + // hardware::device::Trajectory + // up_stairs_leg_lift; + // hardware::device::Trajectory + // up_stairs_leg_press_and_lift; + bool initial_check_done_ = false; + hardware::hsm::up_stairs_hsm::Auto_Leg_Up_Two_Stairs up_stairs; +}; +} // namespace rmcs_core::controller::leg +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::leg::LegController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp new file mode 100644 index 00000000..f17c35a1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/matrix_pid_calculator.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include +#include + +#include + +namespace rmcs_core::controller::pid { + +template +class MatrixPidCalculator { + using Vector = Eigen::Vector; + using Gain = std::conditional, double>::type; + +public: + MatrixPidCalculator(Gain kp, Gain ki, Gain kd) + : kp(std::move(kp)) + , ki(std::move(ki)) + , kd(std::move(kd)) { + integral_min.setConstant(-inf); + integral_max.setConstant(inf); + output_min.setConstant(-inf); + output_max.setConstant(inf); + reset(); + } + + virtual ~MatrixPidCalculator() = default; + + void reset() { + last_err_.setConstant(nan); + err_integral_ = Vector::Zero(); + } + + Vector update(Vector err) { + Vector control = kp * err + ki * err_integral_; + err_integral_ = exclude_nan(clamp(err_integral_ + err, integral_min, integral_max)); + + control += exclude_nan(kd * (err - last_err_)); + last_err_ = err; + + return clamp(control, output_min, output_max); + } + + Gain kp, ki, kd; + Vector integral_min, integral_max; + Vector output_min, output_max; + +private: + static constexpr double inf = std::numeric_limits::infinity(); + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + static auto clamp(const Vector& value, const Vector& min, const Vector& max) { + return value.cwiseMax(min).cwiseMin(max); + } + + static auto exclude_nan(const Vector& value) { + return value.unaryExpr([](double v) { return std::isnan(v) ? 0.0 : v; }); + } + + Vector last_err_, err_integral_; +}; + +} // namespace rmcs_core::controller::pid \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp index 6741bcdb..4eba4ff4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_calculator.hpp @@ -9,6 +9,8 @@ namespace rmcs_core::controller::pid { class PidCalculator { public: + PidCalculator() { reset(); }; + PidCalculator(double kp, double ki, double kd) : kp(kp) , ki(ki) @@ -27,8 +29,13 @@ class PidCalculator { if (!std::isfinite(err)) { return nan; } else { - double control = kp * err + ki * err_integral_; - err_integral_ = std::clamp(err_integral_ + err, integral_min, integral_max); + double control = kp * err; + + if (err < integral_split_max && err > integral_split_min) { + control += ki * err_integral_; + err_integral_ = std::clamp(err_integral_ + err, integral_min, integral_max); + } else + err_integral_ = 0; if (!std::isnan(last_err_)) control += kd * (err - last_err_); @@ -40,9 +47,10 @@ class PidCalculator { double kp, ki, kd; double integral_min = -inf, integral_max = inf; + double integral_split_min = -inf, integral_split_max = inf; double output_min = -inf, output_max = inf; -protected: +private: static constexpr double inf = std::numeric_limits::infinity(); static constexpr double nan = std::numeric_limits::quiet_NaN(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp index 9ae570b1..9780edd3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/pid_controller.cpp @@ -3,7 +3,8 @@ #include #include -#include "pid_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "controller/pid/smart_input.hpp" namespace rmcs_core::controller::pid { @@ -15,40 +16,34 @@ class PidController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , measurement_(*this, "measurement") + , setpoint_(*this, "setpoint") + , feedforward_(*this, "feedforward", 0.0) , pid_calculator_( get_parameter("kp").as_double(), get_parameter("ki").as_double(), get_parameter("kd").as_double()) { - register_input(get_parameter("measurement").as_string(), measurement_); - - // Allows using immediate value instead of message name - auto parameter_setpoint = get_parameter("setpoint"); - if (parameter_setpoint.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { - setpoint_immediate_value_ = parameter_setpoint.as_double(); - setpoint_.bind_directly(setpoint_immediate_value_); - } else { - register_input(parameter_setpoint.as_string(), setpoint_); - } - register_output(get_parameter("control").as_string(), control_); get_parameter("integral_min", pid_calculator_.integral_min); get_parameter("integral_max", pid_calculator_.integral_max); + + get_parameter("integral_split_min", pid_calculator_.integral_split_min); + get_parameter("integral_split_max", pid_calculator_.integral_split_max); + get_parameter("output_min", pid_calculator_.output_min); get_parameter("output_max", pid_calculator_.output_max); } void update() override { - auto err = *setpoint_ - *measurement_; - *control_ = pid_calculator_.update(err); + auto err = *setpoint_ - *measurement_; + *control_ = *feedforward_ + pid_calculator_.update(err); } private: - PidCalculator pid_calculator_; + SmartInput measurement_, setpoint_, feedforward_; - InputInterface measurement_; - InputInterface setpoint_; - double setpoint_immediate_value_; + PidCalculator pid_calculator_; OutputInterface control_; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/smart_input.hpp b/rmcs_ws/src/rmcs_core/src/controller/pid/smart_input.hpp new file mode 100644 index 00000000..a8a843c5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/smart_input.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include + +#include +#include + +namespace rmcs_core::controller::pid { +class SmartInput { +public: + template + requires std::is_base_of_v && std::is_base_of_v + explicit SmartInput(T& component, const std::string& name) { + if (!component.has_parameter(name)) + throw std::runtime_error( + fmt::format( + "Parameter '{}' was not found in component '{}'", name, + component.get_component_name())); + + register_input(component, component.get_parameter(name)); + } + + template + requires std::is_base_of_v && std::is_base_of_v + explicit SmartInput(T& component, const std::string& name, double default_value) { + if (!component.has_parameter(name)) { + input_.make_and_bind_directly(default_value); + return; + } + + register_input(component, component.get_parameter(name)); + } + + double operator*() const { return is_negative_ ? -*input_ : *input_; } + +private: + void register_input(rmcs_executor::Component& component, const rclcpp::Parameter& value) { + if (value.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) { + input_.make_and_bind_directly(value.as_double()); + } else { + const auto& input_name = value.as_string(); + if (input_name[0] == '-') { + is_negative_ = true; + component.register_input(input_name.substr(1), input_); + } else + component.register_input(input_name, input_); + } + } + + rmcs_executor::Component::InputInterface input_; + bool is_negative_ = false; +}; +} // namespace rmcs_core::controller::pid \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/test/Kinematic_test.cpp b/rmcs_ws/src/rmcs_core/src/controller/test/Kinematic_test.cpp new file mode 100644 index 00000000..f80731bd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/test/Kinematic_test.cpp @@ -0,0 +1,40 @@ +#include +#include +using namespace Eigen; + + + +class Kinematic{ + +private: + //a1 alpha1实际上表示的是MDH法中的a0 alpha0，只不过为了传参更简便，所以使用了1.看起来比较统一 + double a1=0,alpha1=0,d1=0.05985,offset1=0; + double a2=0,alpha2=std::numbers::pi/2,d2=0,offset2=std::numbers::pi/2; + double a3=0.41,alpha3=0,d3=0,offset3=0; + double a4=-0.08307,alpha4=std::numbers::pi/2,d4=0.33969,offset4=0; + double a5=0,alpha5=std::numbers::pi/2,d5=0,offset5=std::numbers::pi/2; + double a6=0,alpha6=std::numbers::pi/2,d6=-0.0571,offset6=0; + + + +public: + Matrix positive_kinematic(double theta1,double theta2,double theta3,double theta4,double theta5,double theta6){ + Matrix4d T01,T12,T23,T34,T45,T56; + + T01 << cos(theta1+offset1), -sin(theta1+offset1), 0, a1, + sin(theta1+offset1)*cos(alpha1), cos(theta1+offset1)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d1, + sin(theta1+offset1)*sin(alpha1), cos(theta1+offset1)*sin(alpha1), cos(alpha1), cos(alpha1)*d1, + 0, 0, 0, 1; + + + } + + + + + + + + + +}; diff --git a/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp new file mode 100644 index 00000000..126eb4f4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/low_pass_filter.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include + +#include + +#include + +namespace rmcs_core::filter { + +template +requires(variable_number > 0) class LowPassFilter { + using Value = + std::conditional_t>; + + static auto get_nan() { + if constexpr (variable_number == 1) + return std::numeric_limits::quiet_NaN(); + else + return Eigen::Vector::Constant( + std::numeric_limits::quiet_NaN()); + } + + static void exclude_nan(double& output, double input) { + output = std::isnan(output) ? input : output; + } + + template + static void + exclude_nan(Eigen::Vector& output, const Eigen::Vector& input) { + output = (output.array().isNaN()).select(input, output); + } + +public: + explicit LowPassFilter(double alpha) + : alpha_(alpha) + , previous_output_(get_nan()) {} + + LowPassFilter(double cutoff_frequency, double sampling_frequency) + : previous_output_(get_nan()) { + set_cutoff(cutoff_frequency, sampling_frequency); + } + + void reset() { previous_output_ = get_nan(); } + + Value update(const Value& input) { + Value output = alpha_ * input + (1.0 - alpha_) * previous_output_; + exclude_nan(output, input); + previous_output_ = output; + return output; + } + + void set_alpha(double alpha) { alpha_ = alpha; } + + void set_cutoff(double cutoff_freq, double sampling_freq) { + double dt = 1.0 / sampling_freq; + double rc = 1.0 / (2 * std::numbers::pi * cutoff_freq); + alpha_ = dt / (dt + rc); + } + +private: + double alpha_; + Value previous_output_; +}; + +} // namespace rmcs_core::filter \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/Kinematic.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/Kinematic.hpp new file mode 100644 index 00000000..9b04059f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/Kinematic.hpp @@ -0,0 +1,294 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; +class Kinematic : public rclcpp::Node { +public: + explicit Kinematic(Component& status_component) + : Node("a") { + status_component.register_input("/arm/Joint1/T", T_01); + status_component.register_input("/arm/Joint2/T", T_12); + status_component.register_input("/arm/Joint3/T", T_23); + status_component.register_input("/arm/Joint4/T", T_34); + status_component.register_input("/arm/Joint5/T", T_45); + status_component.register_input("/arm/Joint6/T", T_56); + + // status_component.register_input("/arm/Joint1/d", link_length1); + // status_component.register_input("/arm/Joint2/a", link_length2); + // status_component.register_input("/arm/Joint3/a", link_length3); + // status_component.register_input("/arm/Joint4/d", link_length4); + // status_component.register_input("/arm/Joint6/d", link_length5); + + // status_component.register_input("/arm/Joint1/qlim_up", joint1_qlim_up); + // status_component.register_input("/arm/Joint2/qlim_up", joint2_qlim_up); + // status_component.register_input("/arm/Joint3/qlim_up", joint3_qlim_up); + // status_component.register_input("/arm/Joint4/qlim_up", joint4_qlim_up); + // status_component.register_input("/arm/Joint5/qlim_up", joint5_qlim_up); + // status_component.register_input("/arm/Joint6/qlim_up", joint6_qlim_up); + + // status_component.register_input("/arm/Joint1/qlim_low", joint1_qlim_low); + // status_component.register_input("/arm/Joint2/qlim_low", joint2_qlim_low); + // status_component.register_input("/arm/Joint3/qlim_low", joint3_qlim_low); + // status_component.register_input("/arm/Joint4/qlim_low", joint4_qlim_low); + // status_component.register_input("/arm/Joint5/qlim_low", joint5_qlim_low); + // status_component.register_input("/arm/Joint6/qlim_low", joint6_qlim_low); + } + Kinematic(const Kinematic&) = delete; + Kinematic& operator=(const Kinematic&) = delete; + + void positive_kinematic() { + + // // double yy = (*T_01)(3,3); + // Eigen::Matrix4d T_02 = *T_01 * (*T_12); + // // Eigen::Matrix4d T_03 = T02 + // // Eigen::Matrix4d T_04 + // // Eigen::Matrix4d T_05 + Eigen::Matrix4d T_06 = *T_01 * (*T_12 * (*T_23 * (*T_34 * (*T_45 * (*T_56))))); + x = T_06(0, 3); + y = T_06(1, 3); + z = T_06(2, 3); + // RCLCPP_INFO(this->get_logger(),"%f %f %f %f",T_02(1,0),T_02(1,1),T_02(1,2),T_02(1,3)); + + if (fabs(fabs(T_06(0, 2)) - 1.0) < std::numeric_limits::epsilon()) { + roll = 0; + if (T_06(0, 2) > 0) { + yaw = atan2(T_06(2, 1), T_06(1, 1)); + } else { + yaw = -atan2(T_06(1, 0), T_06(2, 0)); + } + pitch = asin(T_06(0, 2)); + } else { + roll = -atan2(T_06(0, 1), T_06(0, 0)); + yaw = -atan2(T_06(1, 2), T_06(2, 2)); + pitch = atan(T_06(0, 2) * cos(roll) / T_06(0, 0)); + } + } + + double get_x() const { return x; } + double get_y() const { return y; } + double get_z() const { return z; } + double get_roll(bool in_degrees = false) const { + return in_degrees ? roll * (180.0 / M_PI) : roll; + } + double get_yaw(bool in_degrees = false) const { + return in_degrees ? yaw * (180.0 / M_PI) : yaw; + } + double get_pitch(bool in_degrees = false) const { + return in_degrees ? pitch * (180.0 / M_PI) : pitch; + } + + static std::array arm_inverse_kinematic(std::array xyz_rpy) { + double theta1, theta2, theta3 = 0.0, theta4, theta5, theta6; + // static double L_fake = sqrt(*link_length3 * (*link_length3) + *link_length4 * + // (*link_length4)); + double roll = xyz_rpy[3]; + + double pitch = xyz_rpy[4]; + double yaw = xyz_rpy[5]; + static double L_fake = 0.349699; + static double beta = 0.239839; + Eigen::Matrix4d T_R = getTransformationMatrix(xyz_rpy); + double x_e = T_R(0, 3) - T_R(0, 2) * (link_length5); + double y_e = T_R(1, 3) - T_R(1, 2) * (link_length5); + double z_e = T_R(2, 3) - T_R(2, 2) * (link_length5); + // theta1 + double theta1_1 = -atan2(-y_e, x_e); + double theta1_2 = -atan2(-y_e, x_e) + std::numbers::pi; + if (theta1_1 >= joint1_qlim[0] && theta1_1 <= joint1_qlim[1]) + theta1 = theta1_1; + else + theta1 = theta1_2; + // theta2 + double A = x_e / cos(theta1); + double B = z_e - link_length1; + double k21 = -2.0 * A * (link_length2); + double k22 = -2.0 * B * (link_length2); + double d2 = A * A + B * B + (link_length2) * (link_length2)-L_fake * L_fake; + double theta2_1 = atan2(k22, k21) - atan2(-d2, sqrt(k21 * k21 + k22 * k22 - d2 * d2)); + double theta2_2 = atan2(k22, k21) - atan2(-d2, -sqrt(k21 * k21 + k22 * k22 - d2 * d2)); + theta2_1 = normalizeAngle(theta2_1); + theta2_2 = normalizeAngle(theta2_2); + // RCLCPP_INFO( + // this->get_logger(), "%f %f %f %f %f", x_e,y_e, T_R(1, 2), + // theta2_1 * 180 / std::numbers::pi, theta2_2 * 180 / std::numbers::pi); + // theta3 + double d31 = (A + (link_length2)*sin(theta2_1)) / L_fake; + double d32 = (A + (link_length2)*sin(theta2_2)) / L_fake; + double theta3_1_1 = + atan2(cos(theta2_1 - beta), sin(theta2_1 - beta)) - atan2(d31, sqrt(1 - d31 * d31)); + double theta3_1_2 = + atan2(cos(theta2_1 - beta), sin(theta2_1 - beta)) - atan2(d31, -sqrt(1 - d31 * d31)); + double theta3_2_1 = + atan2(cos(theta2_2 - beta), sin(theta2_2 - beta)) - atan2(d32, sqrt(1 - d32 * d32)); + double theta3_2_2 = + atan2(cos(theta2_2 - beta), sin(theta2_2 - beta)) - atan2(d32, -sqrt(1 - d32 * d32)); + + theta3_1_1 = normalizeAngle(theta3_1_1); + theta3_1_2 = normalizeAngle(theta3_1_2); + theta3_2_1 = normalizeAngle(theta3_2_1); + theta3_2_2 = normalizeAngle(theta3_2_2); + if (theta2_1 >= (joint2_qlim[0]) && theta2_1 <= (joint2_qlim[1])) { + theta2 = theta2_1; + if (theta3_1_1 >= (joint3_qlim[0]) && theta3_1_1 <= (joint3_qlim[1])) + theta3 = theta3_1_1; + else if (theta3_1_2 >= (joint3_qlim[0]) && theta3_1_2 <= (joint3_qlim[1])) + theta3 = theta3_1_2; + + else { + if (theta2_2 >= (joint2_qlim[0]) && theta2_2 <= (joint2_qlim[1])) { + theta2 = theta2_2; + if (theta3_2_1 >= (joint3_qlim[0]) && theta3_2_1 <= (joint3_qlim[1])) + theta3 = theta3_2_1; + if (theta3_2_2 >= (joint3_qlim[0]) && theta3_2_2 <= (joint3_qlim[1])) + theta3 = theta3_2_2; + else + theta3 = NAN; + } + } + } else if (theta2_2 >= (joint2_qlim[0]) && theta2_2 <= (joint2_qlim[1])) { + theta2 = theta2_2; + if (theta3_2_1 >= (joint3_qlim[0]) && theta3_2_1 <= (joint3_qlim[1])) + theta3 = theta3_2_1; + if (theta3_2_2 >= (joint3_qlim[0]) && theta3_2_2 <= (joint3_qlim[1])) + theta3 = theta3_2_2; + else + theta3 = NAN; + } else { + theta2 = NAN; + theta3 = NAN; + } + + double r11 = + (sin(theta1) * (cos(yaw) * sin(roll) + cos(roll) * sin(pitch) * sin(yaw)) + * cos(theta2 + theta3)) + + ((sin(roll) * sin(yaw) - cos(roll) * cos(yaw) * sin(pitch)) * (sin(theta2 + theta3))) + + (cos(pitch) * cos(roll) * cos(theta1) * cos(theta2 + theta3)); + double r12 = + (sin(theta1) * (cos(roll) * cos(yaw) - sin(pitch) * sin(roll) * sin(yaw)) + * cos(theta2 + theta3)) + + ((cos(roll) * sin(yaw) + cos(yaw) * sin(pitch) * sin(roll)) * (sin(theta2 + theta3))) + - (cos(pitch) * cos(theta1) * sin(roll) * cos(theta2 + theta3)); + double r13 = (cos(theta1) * sin(pitch) * cos(theta2 + theta3)) + + (cos(pitch) * cos(yaw) * (sin(theta2 + theta3))) + - (cos(pitch) * sin(theta1) * sin(yaw) * cos(theta2 + theta3)); + double r23 = -(sin(pitch) * sin(theta1)) - (cos(pitch) * cos(theta1) * sin(yaw)); + double r33 = (cos(theta1) * sin(pitch) * (-sin(theta2 + theta3))) + + (cos(pitch) * cos(yaw) * cos(theta2 + theta3)) + + (cos(pitch) * sin(theta1) * sin(yaw) * (sin(theta2 + theta3))); + + theta5 = atan2(r13, sqrt(r11 * r11 + r12 * r12)); + theta4 = atan2(-r23 / cos(theta5), r33 / cos(theta5)); + theta6 = atan2(-r12 / cos(theta5), r11 / cos(theta5)); + + theta5 = normalizeAngle(theta5) + std::numbers::pi / 2.0; + theta4 = normalizeAngle(theta4); + theta6 = normalizeAngle(theta6); + if (theta4 > 3.141592 || theta4 < -3.141592) + theta4 = NAN; + if (theta5 > joint5_qlim[1] || theta5 < joint5_qlim[0]) + theta5 = NAN; + if (theta6 > 3.141592 || theta6 < -3.141592) + theta6 = NAN; + return {theta1, theta2, theta3, theta4, theta5, theta6}; + } + +private: + static Eigen::Matrix4d getTransformationMatrix(std::array xyz_rpy_) { + + Eigen::Matrix3d Rz, Ry, Rx, Rotation; + Rz << cos(xyz_rpy_[5]), -sin(xyz_rpy_[5]), 0, sin(xyz_rpy_[5]), cos(xyz_rpy_[5]), 0, 0, 0, + 1; + Ry << cos(xyz_rpy_[4]), 0, sin(xyz_rpy_[4]), 0, 1, 0, -sin(xyz_rpy_[4]), 0, + cos(xyz_rpy_[4]); + Rx << 1, 0, 0, 0, cos(xyz_rpy_[3]), -sin(xyz_rpy_[3]), 0, sin(xyz_rpy_[3]), + cos(xyz_rpy_[3]); + Rotation = Rx * Ry * Rz; + Eigen::Matrix4d transformation; + transformation << Rotation(2, 2), Rotation(1, 2), Rotation(0, 2), xyz_rpy_[0], + Rotation(2, 1), Rotation(1, 1), Rotation(0, 1), xyz_rpy_[1], Rotation(2, 0), + Rotation(1, 0), Rotation(0, 0), xyz_rpy_[2], 0, 0, 0, 1; + return transformation; + } + + // Eigen::Matrix transform_martix (double real_theta ,double alpha,double a,double + // d) + // { + // Eigen::Matrix T_ ; + // T_ << cos(real_theta), -sin(real_theta) * cos(alpha), sin(real_theta) * sin(alpha), + // a * cos(real_theta), sin(real_theta), cos(real_theta) * cos(alpha), + // -cos(real_theta) * sin(alpha), a * sin(real_theta), 0, sin(alpha), cos(alpha), d, + // 0, 0, 0, 1; + // return T_; + // } + static double normalizeAngle(double angle) { + + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + + Component::InputInterface T_01; + Component::InputInterface T_12; + Component::InputInterface T_23; + Component::InputInterface T_34; + Component::InputInterface T_45; + Component::InputInterface T_56; + + // Component::InputInterface link_length1; + // Component::InputInterface link_length2; + // Component::InputInterface link_length3; + // Component::InputInterface link_length4; + // Component::InputInterface link_length5; + + // Component::InputInterface joint1_qlim_up; + // Component::InputInterface joint2_qlim_up; + // Component::InputInterface joint3_qlim_up; + // Component::InputInterface joint4_qlim_up; + // Component::InputInterface joint5_qlim_up; + // Component::InputInterface joint6_qlim_up; + + // Component::InputInterface joint1_qlim_low; + // Component::InputInterface joint2_qlim_low; + // Component::InputInterface joint3_qlim_low; + // Component::InputInterface joint4_qlim_low; + // Component::InputInterface joint5_qlim_low; + // Component::InputInterface joint6_qlim_low; + static constexpr double link_length1 = 0.05985; + static constexpr double link_length2 = 0.41; + static constexpr double link_length3 = -0.08307; + static constexpr double link_length4 = 0.33969; + static constexpr double link_length5 = -0.0571; + + static constexpr std::array joint1_qlim = {-3.141592, 3.141592}; + static constexpr std::array joint2_qlim = {-1.308, 1.16719}; + static constexpr std::array joint3_qlim = {-1.0472, 0.8727}; + static constexpr std::array joint4_qlim = {-3.141592, 3.141592}; + static constexpr std::array joint5_qlim = {-1.93532, 1.93532}; + static constexpr std::array joint6_qlim = {-3.141592, 3.141592}; + + double x; + double y; + double z; + double roll; + double yaw; + double pitch; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp index d37e2c5a..da5dee42 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dji_motor.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -51,7 +52,7 @@ struct DjiMotorConfig { DjiMotorConfig& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } }; -class DjiMotor { +class DjiMotor{ public: DjiMotor( Component& status_component, Component& command_component, const std::string& name_prefix) { @@ -135,6 +136,7 @@ class DjiMotor { // Angle unit: rad int angle = raw_angle - encoder_zero_point_; + raw_angle_ = raw_angle; if (angle < 0) angle += raw_angle_max_; if (!multi_turn_angle_enabled_) { @@ -185,6 +187,7 @@ class DjiMotor { double get_velocity() { return *velocity_; } double get_torque() { return *torque_; } double get_max_torque() { return *max_torque_; } + int get_raw_angle() const { return raw_angle_; } private: struct alignas(uint64_t) DjiMotorFeedback { @@ -201,6 +204,7 @@ class DjiMotor { std::atomic can_data_ = 0; + int raw_angle_; static constexpr int raw_angle_max_ = 8192; int encoder_zero_point_, last_raw_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp new file mode 100644 index 00000000..be47079e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -0,0 +1,202 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/endian_promise.hpp" + +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; + +enum class DMMotorType : uint8_t { UNKNOWN = 0, DM8009 }; + +struct DMMotorConfig { + DMMotorType motor_type; + int encoder_zero_point; // 0~65536 + double gear_ratio; + double reversed; + bool multi_turn_angle_enabled; + double iq; + + explicit DMMotorConfig(DMMotorType motor_type) { + this->encoder_zero_point = 0; + this->motor_type = motor_type; + this->reversed = 1.0; + this->multi_turn_angle_enabled = false; + this->gear_ratio = 1.0; + } + DMMotorConfig& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + DMMotorConfig& set_gear_ratio(double value) { return gear_ratio = value, *this; } + DMMotorConfig& reverse() { return reversed = -1.0, *this; } + DMMotorConfig& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } +}; + +class DMMotor : rclcpp::Node { +public: + DMMotor( + Component& status_component, Component& command_component, const std::string& name_prefix) + : rclcpp::Node("agfdhfhf") + + { + encoder_zero_point_ = 0; + last_raw_angle_ = 0; + multi_turn_angle_enabled_ = false; + + raw_angle_to_angle_coefficient_ = angle_to_raw_angle_coefficient_ = 0.0; + raw_current_to_torque_coefficient_ = torque_to_raw_current_coefficient_ = 0.0; + + status_component.register_output(name_prefix + "/angle", angle_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_, 0.0); + + status_component.register_output(name_prefix + "/motor", motor_, this); + + command_component.register_input(name_prefix + "/control_torque", control_torque_); + } + DMMotor(const DMMotor&) = delete; + DMMotor& operator=(const DMMotor&) = delete; + + void configure(const DMMotorConfig& config) { + + switch (config.motor_type) { + case DMMotorType::DM8009: + VMAX = 25.0; + TMAX = 40.0; + break; + + default: throw std::runtime_error{"Unknown motor type"}; + } + reverse = config.reversed; + gear_ratio_ = config.gear_ratio; + + encoder_zero_point_ = config.encoder_zero_point % (raw_angle_max_); + if (encoder_zero_point_ < 0) + encoder_zero_point_ += (raw_angle_max_); + + raw_angle_to_angle_coefficient_ = + 1.0 * reverse * 2 * std::numbers::pi / (raw_angle_max_ * gear_ratio_); + angle_to_raw_angle_coefficient_ = 1.0 / raw_angle_to_angle_coefficient_; + + raw_velocity_to_velocity_coefficient_ = + 1.0 * (reverse * 60.0) / (2 * std::numbers::pi * gear_ratio_); + velocity_to_raw_velocity_coefficient_ = 1.0 / raw_velocity_to_velocity_coefficient_; + + raw_current_to_torque_coefficient_ = 1.0 * config.gear_ratio; + torque_to_raw_current_coefficient_ = 1.0 / raw_current_to_torque_coefficient_; + + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + angle_multi_turn_ = 0; + } + + void store_status(uint64_t can_result) { + + can_result_.store(can_result, std::memory_order::relaxed); + } + void update() { + uint64_t value = can_result_.load(std::memory_order_relaxed); + + uint8_t rx_buff[8]; + std::memcpy(rx_buff, &value, sizeof(value)); + *id = (rx_buff[0]) & 0xff; + *state = (rx_buff[0]) >> 4; + uint16_t p_int = (rx_buff[1] << 8) | rx_buff[2]; + uint16_t v_int = (rx_buff[3] << 4) | (rx_buff[4] >> 4); + uint16_t t_int = ((rx_buff[4] & 0xF) << 8) | rx_buff[5]; + uint16_t angle = p_int - encoder_zero_point_; + if (angle < 0) + angle += raw_angle_max_; + *angle_ = angle < raw_angle_max_ / 2 + ? raw_angle_to_angle_coefficient_ * angle + : -2 * std::numbers::pi + raw_angle_to_angle_coefficient_ * angle; + *velocity_ = uint_to_double(v_int, -VMAX, VMAX, 12); + *torque_ = uint_to_double(t_int, -TMAX, TMAX, 12); + + *T_mos = (double)(rx_buff[6]); + *T_coil = (double)(rx_buff[7]); + } + + uint64_t generate_torque_command() { + uint64_t result; + double torque = *control_torque_; + if (std::isnan(torque)) { + torque = 0.0; + } + torque = std::clamp(torque, -TMAX, TMAX); + uint16_t tor_tmp = double_to_uint(torque, -TMAX, TMAX, 12); + uint8_t tx_buff[8] = {0, 0, 0, 0, 0, 0, 0, 0}; + tx_buff[6] = ((0 & 0xf) << 4) | (tor_tmp >> 8); + tx_buff[7] = tor_tmp; + std::copy(tx_buff, tx_buff + 8, reinterpret_cast(&result)); + return result; + } + + static uint64_t dm_enable_command() { return std::bit_cast(DM_ENABLE); } + static uint64_t dm_close_command() { return std::bit_cast(DM_CLOSE); } + static uint64_t dm_clear_error_command() { return std::bit_cast(DM_CLEAR_ERROR); } + + double get_angle() { return *angle_; } + double get_velocity() { return *velocity_; } + double get_torque() { return *torque_; } + double get_state() { return *state; } + double get_T_coil() { return *T_coil; } + +private: + std::atomic can_result_ = 0; + + static double uint_to_double(int x_int, double x_min, double x_max, int bits) { + double span = x_max - x_min; + double offset = x_min; + return ((double)x_int) * span / ((double)((1 << bits) - 1)) + offset; + } + static int double_to_uint(double x_double, double x_min, double x_max, int bits) { + double span = x_max - x_min; + double offset = x_min; + return (int)((x_double - offset) * ((double)((1 << bits) - 1)) / span); + } + + int last_raw_angle_; + + double reverse = 1.0; + double gear_ratio_ = 1.0; + + int encoder_zero_point_; + bool multi_turn_angle_enabled_; + int64_t angle_multi_turn_; + + double raw_angle_to_angle_coefficient_, angle_to_raw_angle_coefficient_; + double raw_velocity_to_velocity_coefficient_, velocity_to_raw_velocity_coefficient_; + double raw_current_to_torque_coefficient_, torque_to_raw_current_coefficient_; + + double VMAX, TMAX; + static constexpr uint8_t DM_ENABLE[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC}; + static constexpr uint8_t DM_CLOSE[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD}; + static constexpr uint8_t DM_CLEAR_ERROR[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB}; + + static constexpr uint16_t raw_angle_max_ = 65535; + Component::OutputInterface id; + Component::OutputInterface state; + Component::OutputInterface angle_; + Component::OutputInterface velocity_; + Component::OutputInterface torque_; + Component::OutputInterface T_mos; + Component::OutputInterface T_coil; + + Component::OutputInterface motor_; + + Component::InputInterface control_torque_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/drag_teach.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/drag_teach.hpp new file mode 100644 index 00000000..6ce9e08e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/drag_teach.hpp @@ -0,0 +1,60 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller { + +class Drag : public rclcpp::Node { +public: + explicit Drag(const std::string& filename) + : rclcpp::Node("Drag") { + filename_ = filename; + infile.open(filename, std::ios::binary); + if (infile) { + RCLCPP_INFO(this->get_logger(), "open file successfully"); + is_file_open = true; + } else { + is_file_open = false; + } + } + + void read_data_from_file() { + if (is_file_open) { + infile.read(reinterpret_cast(data.data()), sizeof(data)); + } else { + for (auto& value : data) { + value = NAN; + } + } + } + void write_data_to_file(const std::array& data) { + std::ofstream outfile(filename_, std::ios::binary | std::ios::app); + if (outfile) { + outfile.write(reinterpret_cast(data.data()), sizeof(data)); + } else { + } + } + void reset_file_pointer() { + if (infile) { + infile.clear(); + infile.seekg(0, std::ios::beg); + } + } + + const double* get_data() const { return data.data(); } + +private: + + bool is_file_open = false; + std::array data; + std::ifstream infile; + std::string filename_; +}; + +} // namespace rmcs_core::controller \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/encorder.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/encorder.hpp new file mode 100644 index 00000000..6e18a443 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/encorder.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; +struct EncoderConfig { + + int encoder_zero_point; + double reversed; + bool multi_turn_angle_enabled; + + explicit EncoderConfig() { + this->encoder_zero_point = 0; + this->reversed = 1.0; + this->multi_turn_angle_enabled = false; + + } + EncoderConfig& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + EncoderConfig& reverse() { return reversed = -1.0, *this; } + EncoderConfig& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } +}; +class Encoder //: public rclcpp::Node +{ +public: + Encoder(Component& status_component, const std::string& name_prefix) + //:Node{"aaa"} + { + status_component.register_output(name_prefix + "/angle", angle_, 0.0); + status_component.register_output(name_prefix + "/encoder", encoder_, this); + } + + Encoder(const Encoder&) = delete; + Encoder& operator=(const Encoder&) = delete; + + + void configure(const EncoderConfig& config){ + encoder_zero_point_ = config.encoder_zero_point % raw_angle_max_; + reverse = config.reversed; + raw_angle_to_angle_coefficient_ = + (1.0/ (raw_angle_max_))* 2.0 * std::numbers::pi; + angle_to_raw_angle_coefficient_ = 1.0 / raw_angle_to_angle_coefficient_; + + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + angle_multi_turn_ = 0; + } + void store_status(uint64_t can_result) { + raw_angle = std::bitset<18>(can_result); + } + void update() { + int raw_angle_ = static_cast(raw_angle.to_ulong()); + + int angle = raw_angle_ - encoder_zero_point_; + if(angle < 0) angle+=raw_angle_max_; + if (!multi_turn_angle_enabled_) { + *angle_ = this->reverse*(((double)angle <= (raw_angle_max_)/2.0 )? (raw_angle_to_angle_coefficient_ * angle): (-2*std::numbers::pi )+ (raw_angle_to_angle_coefficient_ * angle)); + } else { + auto diff = (angle - angle_multi_turn_) % (raw_angle_max_); + if (diff <= -(raw_angle_max_)/ 2) + diff += (raw_angle_max_); + else if (diff > (raw_angle_max_)/ 2) + diff -= (raw_angle_max_); + angle_multi_turn_ += diff; + *angle_ = this->reverse * raw_angle_to_angle_coefficient_ * static_cast(angle_multi_turn_); + last_raw_angle_ = raw_angle_; + } + } + double get_angle() { return *angle_; } + int get_raw_angle() { return static_cast(raw_angle.to_ulong());} + +private: + + std::bitset<18> raw_angle; + int64_t angle_multi_turn_; + Component::OutputInterface angle_; + Component::OutputInterface encoder_; + + double raw_angle_to_angle_coefficient_, angle_to_raw_angle_coefficient_; + int encoder_zero_point_; + static constexpr int raw_angle_max_ = 262144; + int last_raw_angle_; + double reverse; + Component::OutputInterface gear_ratio_; + bool multi_turn_angle_enabled_;}; + + + +} \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/joint.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/joint.hpp new file mode 100644 index 00000000..e66d7508 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/joint.hpp @@ -0,0 +1,150 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/endian_promise.hpp" +#include +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; + +struct DHConfig { + double a_, d_, alpha_, offset_; + double theta_source; + bool is_change_theta_feedback_; + + explicit DHConfig(double a, double d, double alpha, double offset) { + + this->a_ = a; + this->d_ = d; + this->alpha_ = alpha; + this->offset_ = offset; + } +}; + +struct Qlim_Stall_Config { + // todo::add Stall relavant parameters + std::array qlim_; + + explicit Qlim_Stall_Config(const std::vector& qlim_input) { + if (qlim_input.size() != 2) { + throw std::runtime_error("Error: qlim_input must have exactly 2 elements"); + } + if (qlim_input[0] >= qlim_input[1]) { + throw std::runtime_error("Error: qlim[0] should be less than or qlim[1]"); + } + this->qlim_[0] = qlim_input[0]; + this->qlim_[1] = qlim_input[1]; + } +}; + +class Joint + : public LKMotor + { +public: + Joint(Component& status_component, Component& command_component, const std::string& name_prefix) + : LKMotor(status_component, command_component, name_prefix) + // , rclcpp::Node("joint_hpp") + , name_prefix_(name_prefix) { + + status_component.register_output(name_prefix + "/theta", theta, 0.0); + status_component.register_output(name_prefix + "/T", T_, Eigen::Matrix4d::Zero()); + status_component.register_output(name_prefix + "/a", a, 0.0); + status_component.register_output(name_prefix + "/d", d, 0.0); + status_component.register_output(name_prefix + "/alpha", alpha, 0.0); + status_component.register_output(name_prefix + "/offset", offset, 0.0); + status_component.register_output(name_prefix + "/qlim_up", qlim_up, 0.0); + status_component.register_output(name_prefix + "/qlim_low", qlim_low, 0.0); + + command_component.register_input(name_prefix + "/target_theta", target_theta); + + is_change_theta_feedback = false; + theta_source_ = NAN; + } + Joint(const Joint&) = delete; + Joint& operator=(const Joint&) = delete; + Joint& update_joint() { + this->update(); + if (is_change_theta_feedback == false) + *theta = this->get_angle(); + else { + *theta = theta_source_; + } + + double real_theta = *theta + *offset; + *T_ << cos(real_theta), -sin(real_theta) * cos(*alpha), sin(real_theta) * sin(*alpha), + *a * cos(real_theta), sin(real_theta), cos(real_theta) * cos(*alpha), + -cos(real_theta) * sin(*alpha), *a * sin(real_theta), 0, sin(*alpha), cos(*alpha), *d, + 0, 0, 0, 1; + + return *this; + } + Joint& change_theta_feedback_(double value) { + theta_source_ = value; + is_change_theta_feedback = true; + return *this; + }; + + void configure_joint( + const LKMotorConfig& motor_config, const DHConfig& dh_config, + const Qlim_Stall_Config& qlim_stall_config_) { + + this->configure(motor_config); + *a = dh_config.a_; + *d = dh_config.d_; + *alpha = dh_config.alpha_; + *offset = dh_config.offset_; + *qlim_low = qlim_stall_config_.qlim_[0]; + *qlim_up = qlim_stall_config_.qlim_[1]; + } + + double get_theta() { return *theta; } + double get_a() { return *a; } + double get_d() { return *d; } + double get_alpha() { return *alpha; } + double get_offset() { return *offset; } + double get_qlim_up() { return *qlim_up; } + double get_qlim_low() { return *qlim_low; } + double get_target_theta() { + // todo:add stall + double target_theta_ = *target_theta; + target_theta_ = std::clamp(target_theta_, *qlim_low, *qlim_up); + return target_theta_; + } + double get_vel(){return this->get_velocity();} + // Eigen::Matrix4d get_transform() { return *T_; } + +private: + // double a,d,alpha,offset,theta; + bool is_change_theta_feedback; + double theta_source_; + std::string name_prefix_; + + Component::OutputInterface qlim_low; + Component::OutputInterface qlim_up; + Component::OutputInterface theta; + Component::OutputInterface a; + Component::OutputInterface d; + Component::OutputInterface offset; + Component::OutputInterface alpha; + Component::OutputInterface T_; + + Component::InputInterface target_theta; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp new file mode 100644 index 00000000..ba29de1b --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -0,0 +1,304 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "hardware/endian_promise.hpp" + +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; + +enum class LKMotorType : uint8_t { + UNKNOWN = 0, + MF7015V210T = 1, + MG4010E_i10V3 = 2, + MG4010E_i36V3 = 3, + MG8010E_i36 = 4, + MHF7015 = 5, +}; + +struct LKMotorConfig { + LKMotorType motor_type; + int encoder_zero_point; // 0~65536 + double gear_ratio; + double reversed; + bool multi_turn_angle_enabled; + bool is_angle_zero_to_2pi; + double iq; + + explicit LKMotorConfig(LKMotorType motor_type) { + this->encoder_zero_point = 0; + this->motor_type = motor_type; + this->reversed = 1.0; + this->multi_turn_angle_enabled = false; + this->gear_ratio = 1.0; + this->is_angle_zero_to_2pi = false; + switch (motor_type) { + case LKMotorType::UNKNOWN: + case LKMotorType::MG4010E_i10V3: + case LKMotorType::MG4010E_i36V3: + case LKMotorType::MG8010E_i36: iq = 66.0 / 4096; break; + case LKMotorType::MHF7015: + case LKMotorType::MF7015V210T: iq = 33.0 / 4096; break; + } + } + LKMotorConfig& set_encoder_zero_point(int value) { return encoder_zero_point = value, *this; } + LKMotorConfig& set_gear_ratio(double value) { return gear_ratio = value, *this; } + LKMotorConfig& reverse() { return reversed = -1.0, *this; } + LKMotorConfig& enable_multi_turn_angle() { return multi_turn_angle_enabled = true, *this; } + LKMotorConfig& enable_angle_zero_to_2pi() { return is_angle_zero_to_2pi = true, *this; } +}; + +class LKMotor { +public: + LKMotor( + Component& status_component, Component& command_component, const std::string& name_prefix) + + { + encoder_zero_point_ = 0; + last_raw_angle_ = 0; + multi_turn_angle_enabled_ = false; + + raw_angle_to_angle_coefficient_ = angle_to_raw_angle_coefficient_ = 0.0; + raw_current_to_torque_coefficient_ = torque_to_raw_current_coefficient_ = 0.0; + + status_component.register_output(name_prefix + "/gear_ratio", gear_ratio_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + status_component.register_output(name_prefix + "/rate_torque", rated_torque_, 0.0); + + status_component.register_output(name_prefix + "/angle", angle_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_, 0.0); + status_component.register_output(name_prefix + "/raw_angle", raw_angle_, 0); + + status_component.register_output(name_prefix + "/motor", motor_, this); + + command_component.register_input(name_prefix + "/control_torque", control_torque_); + } + LKMotor(const LKMotor&) = delete; + LKMotor& operator=(const LKMotor&) = delete; + + void configure(const LKMotorConfig& config) { + + double torque_constant, rated_current, rated_torque, max_torque; + switch (config.motor_type) { + case LKMotorType::MF7015V210T: + torque_constant = 0.12; + rated_current = 8.3; + rated_torque = 1.0; + max_torque = 2.0; + LSB = 18000; + break; + case LKMotorType::MG4010E_i10V3: + torque_constant = 0.07 * 10; + rated_current = 3.5; + rated_torque = 2.5; + max_torque = 4.5; + LSB = 180000; + break; + case LKMotorType::MG4010E_i36V3: + torque_constant = 2.58; + rated_current = 3.5; + rated_torque = 9; + max_torque = 18; + LSB = 648000; + break; + case LKMotorType::MG8010E_i36: + torque_constant = 0.15 * 36; + rated_current = 6.9; + rated_torque = 35.0; + max_torque = 45.0; + LSB = 648000; + break; + case LKMotorType::MHF7015: + torque_constant = 0.51; + rated_current = 1.93; + rated_torque = 0.99; + max_torque = 2.42; + LSB = 18000; + break; + default: throw std::runtime_error{"Unknown motor type"}; + } + + encoder_zero_point_ = config.encoder_zero_point % (raw_angle_max_); + if (encoder_zero_point_ < 0) + encoder_zero_point_ += (raw_angle_max_); + + reverse = config.reversed; + + raw_angle_to_angle_coefficient_ = + 1.0 * (1.0 / (raw_angle_max_) / config.gear_ratio) * 2.0 * std::numbers::pi; + + angle_to_raw_angle_coefficient_ = 1.0 / raw_angle_to_angle_coefficient_; + + raw_velocity_to_velocity_coefficient_ = 1.0 / (config.gear_ratio * 6); + velocity_to_raw_velocity_coefficient_ = 1.0 / raw_velocity_to_velocity_coefficient_; + + raw_current_to_torque_coefficient_ = // 含有传动比 + 1.0 * config.gear_ratio * torque_constant * config.iq; + torque_to_raw_current_coefficient_ = 1.0 / raw_current_to_torque_coefficient_; + + *gear_ratio_ = config.gear_ratio; + + *max_torque_ = config.gear_ratio * max_torque; + *rated_torque_ = config.gear_ratio * rated_torque; + + multi_turn_angle_enabled_ = config.multi_turn_angle_enabled; + angle_zero_to_2pi_enabled_ = config.is_angle_zero_to_2pi; + angle_multi_turn_ = 0; + } + + void store_status(uint64_t can_result) { + uint8_t command_byte = static_cast(can_result); + if (command_byte == 0x9C || command_byte == 0xA4 || command_byte == 0xA1 + || command_byte == 0xAD) { + can_result_.store(can_result, std::memory_order::relaxed); + } + } + void update() { + auto feedback = + std::bit_cast(can_result_.load(std::memory_order::relaxed)); + int raw_angle = feedback.encoder; + *raw_angle_ = feedback.encoder; + int angle = raw_angle - encoder_zero_point_; + if (angle < 0) + angle += raw_angle_max_; + if (!multi_turn_angle_enabled_) { + if (!angle_zero_to_2pi_enabled_) { + *angle_ = + this->reverse + * (((double)angle <= (raw_angle_max_) / 2.0) + ? (raw_angle_to_angle_coefficient_ * angle) + : (-2 * std::numbers::pi) + (raw_angle_to_angle_coefficient_ * angle)); + } else { + *angle_ = + this->reverse * raw_angle_to_angle_coefficient_ * static_cast(angle); + } + } else { + auto diff = (angle - angle_multi_turn_) % (raw_angle_max_); + if (diff <= -(raw_angle_max_) / 2) + diff += (raw_angle_max_); + else if (diff > (raw_angle_max_) / 2) + diff -= (raw_angle_max_); + angle_multi_turn_ += diff; + *angle_ = this->reverse * raw_angle_to_angle_coefficient_ + * static_cast(angle_multi_turn_); + } + + // Velocity unit: rpm + *velocity_ = raw_velocity_to_velocity_coefficient_ * static_cast(feedback.velocity); + + // Torque unit: N*m + *torque_ = raw_current_to_torque_coefficient_ * static_cast(feedback.current); + + last_raw_angle_ = raw_angle; + } + + uint64_t generate_torque_command() { + std::array result = {0}; + result[0] = 0xA1; + double torque = *control_torque_; + if (std::isnan(torque)) { + result[1] = 0X00; + result[2] = 0X00; + result[3] = 0X00; + result[4] = 0X00; + result[5] = 0X00; + result[6] = 0X00; + result[7] = 0X00; + + return std::bit_cast(result); + ; + } + double max_torque = (*motor_)->get_max_torque(); + torque = std::clamp(torque, -max_torque, max_torque); + double current = std::round((*motor_)->torque_to_raw_current_coefficient_ * torque); + int16_t control_current = static_cast(current); + auto control_current_bits = std::bit_cast>(control_current); + std::copy(control_current_bits.begin(), control_current_bits.end(), result.begin() + 4); + return std::bit_cast(result); + } + + // uint64_t generate_velocity_command(double radians, double max_speed_rpm) { + // std::array result = {0}; + // radians = this->reverse * radians; + // result[0] = 0xAD; + + // uint16_t max_speed = static_cast(max_speed_rpm * 6); + // auto maxSpeedBytes = std::bit_cast>(max_speed); + // //std::copy(maxSpeedBytes.begin(), maxSpeedBytes.end(), result.begin() + 2); + + // double real_radians = radians + this->encoder_zero_point_ * + // raw_angle_to_angle_coefficient_; real_radians = real_radians >= 0 ? real_radians : 2 * + // std::numbers::pi + real_radians; double degrees = real_radians * this->LSB * (* + // motor_)->get_gear_ratio() / std::numbers::pi; uint32_t angle_control = + // static_cast(degrees); auto angleControlBytes = + // std::bit_cast>(angle_control); + // std::copy(angleControlBytes.begin(), angleControlBytes.end(), result.begin() + 4); + + // return std::bit_cast(result); + // } + + static uint64_t lk_stop_command() { return std::bit_cast(uint64_t{0x81}); } + static uint64_t lk_quest_command() { return std::bit_cast(uint64_t{0x9C}); } + static uint64_t lk_enable_command() { return std::bit_cast(uint64_t{0x88}); } + static uint64_t lk_close_command() { return std::bit_cast(uint64_t{0x80}); } + + double get_angle() { return *angle_; } + double get_velocity() { return *velocity_; } + double get_torque() { return *torque_; } + double get_max_torque() { return *max_torque_; } + double get_raw_angle() { return *raw_angle_; } + +private: + struct alignas(uint64_t) LKMotorFeedback { + uint8_t command; + uint8_t temperature; + int16_t current; + int16_t velocity; + uint16_t encoder; + }; + + std::atomic can_result_ = 0; + + static constexpr uint16_t raw_angle_max_ = 65535; + int last_raw_angle_; + uint32_t LSB; + double reverse; + int encoder_zero_point_; + bool multi_turn_angle_enabled_, angle_zero_to_2pi_enabled_; + int64_t angle_multi_turn_; + + double raw_angle_to_angle_coefficient_, angle_to_raw_angle_coefficient_; + double raw_velocity_to_velocity_coefficient_, velocity_to_raw_velocity_coefficient_; + double raw_current_to_torque_coefficient_, torque_to_raw_current_coefficient_; + + Component::OutputInterface gear_ratio_; + Component::OutputInterface max_torque_; + Component::OutputInterface rated_torque_; + + Component::OutputInterface raw_angle_; + Component::OutputInterface angle_; + Component::OutputInterface velocity_; + Component::OutputInterface torque_; + + Component::OutputInterface motor_; + + Component::InputInterface control_torque_; + Component::InputInterface control_velocity; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/power_meter .hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/power_meter .hpp new file mode 100644 index 00000000..421650f3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/power_meter .hpp @@ -0,0 +1,71 @@ +#pragma once +#include "hardware/endian_promise.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; +struct PowerMeterConfig { + explicit PowerMeterConfig() = default; +}; +class PowerMeter { +public: + PowerMeter(Component& status_component, const std::string& name_prefix) { + status_component.register_output(name_prefix + "/voltage", voltage_, 0.0f); + status_component.register_output(name_prefix + "/current", current_, 0.0f); + status_component.register_output(name_prefix + "/power", power_, 0.0f); + } + + PowerMeter(const PowerMeter&) = delete; + PowerMeter& operator=(const PowerMeter&) = delete; + + void store_status(uint64_t can_result) { + can_data_.store(can_result, std::memory_order::relaxed); + } + + void update() { + auto feedback = + std::bit_cast(can_data_.load(std::memory_order::relaxed)); + + *voltage_ = static_cast(feedback.voltage) + * (range_conversion_voltage_ / range_conversion_factor); + *current_ = static_cast(feedback.current) + * (range_conversion_current_ / range_conversion_factor); + *power_ = static_cast(feedback.power) + * (range_conversion_power_ / range_conversion_factor); + if (*power_ >= 120.0f)[[unlikely]] { + // RCLCPP_WARN( + // rclcpp::get_logger("PowerMeter"), "Power reading is abnormally high: %.2f W", + // *power_); + } + } + + double get_voltage() const { return *voltage_; } + double get_current() const { return *current_; } + double get_power() const { return *power_; } + +private: + struct alignas(uint64_t) PowerMeterFeedback { + uint16_t voltage; + uint16_t current; + uint16_t power; + uint16_t unused; + }; + static constexpr int range_conversion_factor = 65535; + static constexpr double range_conversion_voltage_ = 65.535; + static constexpr double range_conversion_current_ = 65.535; + static constexpr double range_conversion_power_ = 650.535; + std::atomic can_data_ = 0; + Component::OutputInterface voltage_; + Component::OutputInterface current_; + Component::OutputInterface power_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/relay.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/relay.hpp new file mode 100644 index 00000000..0539002e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/relay.hpp @@ -0,0 +1,61 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; +enum class RelayType : uint8_t { UNKNOWN = 0, Four_CH = 1, One_CH = 2 }; + +class Relay { +public: +Relay(Component& command_component, const std::string& name_prefix, RelayType type) { + + command_component.register_input(name_prefix + "/CH", CH_); + type_ = type; + } + uint64_t generate_commande() { + std::array result = {0}; + std::vector bits; + + for (int i = 7; i >= 0; --i) { + int bit = (*CH_ >> i) & 1; + bits.push_back(bit); + } + if (type_ == RelayType::Four_CH) { + result[0] = bits[0]; + result[1] = bits[1]; + result[2] = bits[2]; + result[3] = bits[3]; + result[4] = 0; + result[5] = 0; + result[6] = 0; + result[7] = 0; + + } else if (type_ == RelayType::One_CH) { + result[0] = bits[0]; + result[1] = 0; + result[2] = 0; + result[3] = 0; + result[4] = 0; + result[5] = 0; + result[6] = 0; + result[7] = 0; + } + + return std::bit_cast(result); + } + +private: + Component::InputInterface CH_; + RelayType type_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp new file mode 100644 index 00000000..2eaf4b85 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/tof.hpp @@ -0,0 +1,118 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +namespace rmcs_core::hardware::device { +using rmcs_executor::Component; +class Tof : public rclcpp::Node { +public: + explicit Tof(Component& status_component, const std::string& name_prefix) + : Node{"bbb"} { + status_component.register_output( + name_prefix + "/distance", distance_, std::numeric_limits::quiet_NaN()); + + status_component.register_output(name_prefix + "/tof", tof_, this); + }; + + Tof(const Tof&) = delete; + Tof& operator=(const Tof&) = delete; + + void store_status(const std::byte* uart_data, size_t uart_data_length) { + package_.raw_data.resize(uart_data_length); + std::memcpy(package_.raw_data.data(), uart_data, uart_data_length); + } + + void update() { + + bool is_valid = package_.manage_data(); + + if (is_valid) { + + if (get_confidence() > 40) { + + if (!std::isnan(package_.distance_m)) + *distance_ = package_.distance_m; + RCLCPP_INFO(this->get_logger(), "distance:%.5f", *distance_); + } + else{ + *distance_ = std::numeric_limits::quiet_NaN(); + RCLCPP_INFO(this->get_logger(),"unconfident data"); + } + + } else { + *distance_ = std::numeric_limits::quiet_NaN(); + //RCLCPP_INFO(this->get_logger(), "manage data error"); + } + } + + double get_distance() const { + if (!std::isnan(*distance_)) + return *distance_; + return std::numeric_limits::quiet_NaN(); + } + double get_confidence() const { + if (!std::isnan(package_.confidence)) + return package_.confidence; + return std::numeric_limits::quiet_NaN(); + } + +private: + struct TofPackage { + std::vector raw_data; + double distance_m = std::numeric_limits::quiet_NaN(); + double confidence = std::numeric_limits::quiet_NaN(); + + bool manage_data() { + if (raw_data.empty()) + return false; + + std::string ascii_data; + ascii_data.reserve(raw_data.size()); + for (auto b : raw_data) { + ascii_data.push_back(static_cast(b)); + } + + + if (!ascii_data.empty() && ascii_data.back() == '\n') + ascii_data.pop_back(); + + size_t comma = ascii_data.find(','); + if (comma == std::string::npos) + return false; + + std::string distance_str = ascii_data.substr(0, comma); + std::string confidence_str = ascii_data.substr(comma + 1); + + if (!confidence_str.empty() && confidence_str.front() == ' ') + confidence_str.erase(0, 1); + + try { + distance_m = std::stod(distance_str) / 1000.0; + confidence = std::stod(confidence_str); + } catch (...) { + return false; + } + + if (confidence == 0) { + return false; + } + + return true; + } + }; + + TofPackage package_; + Component::OutputInterface distance_; + + Component::OutputInterface tof_; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/trajectory.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/trajectory.hpp new file mode 100644 index 00000000..a00387ab --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/trajectory.hpp @@ -0,0 +1,150 @@ +#pragma once + +#include "cmath" +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::hardware::device { + +enum class TrajectoryType { LINE, BEZIER, JOINT, SINGLE_JOINT }; + +template +class Trajectory +//: public rclcpp::Node + { +public: + using JointArrayType = typename std::conditional< + Type == TrajectoryType::SINGLE_JOINT, std::array, std::array>::type; + JointArrayType joint_start; + JointArrayType joint_end; + Trajectory() + // : Node("trajectory_node") + { + current_step = 1.0f; + is_complete = false; + } + + Trajectory& set_start_point(std::array xyz, std::array rpy) { + xyz_start = xyz; + rpy_start = rpy; + return *this; + } + template < + TrajectoryType T = Type, typename std::enable_if::type = 0> + Trajectory& set_start_point(std::array joint_angles) { + joint_start = joint_angles; + return *this; + } + + + Trajectory& set_end_point(std::array xyz, std::array rpy) { + xyz_end = xyz; + rpy_end = rpy; + return *this; + } + template < + TrajectoryType T = Type, typename std::enable_if::type = 0> + Trajectory& set_end_point(std::array joint_angles) { + joint_end = joint_angles; + return *this; + } + template < + TrajectoryType T = Type, + typename std::enable_if::type = 0> + Trajectory& set_end_point(std::array joint_angle) { + joint_end = joint_angle; + return *this; + } + + Trajectory& set_control_points(std::array xyz1, std::array xyz2) { + if constexpr (Type == TrajectoryType::BEZIER) { + xyz_control_1 = xyz1; + xyz_control_2 = xyz2; + } + return *this; + } + + Trajectory& set_total_step(int total_step_) { + total_step = total_step_; + return *this; + } + + void reset() { + current_step = 1.0f; + is_complete = false; + } + + bool get_complete() const { return is_complete; } + + std::array trajectory() { + double alpha = (current_step - 1) / (total_step - 1); + if (current_step == total_step + 1.0) { + is_complete = true; + // RCLCPP_INFO( + // this->get_logger(), "%f %f %f %f %f %f", result[0], result[1], result[2], + // result[3], result[4], result[5]); + } + + if (current_step <= total_step) { + + if constexpr (Type == TrajectoryType::LINE || Type == TrajectoryType::BEZIER) { + Eigen::Vector3d xyz_result; + if constexpr (Type == TrajectoryType::LINE) { + Eigen::Vector3d xyz_start_eigen(xyz_start[0], xyz_start[1], xyz_start[2]); + Eigen::Vector3d xyz_end_eigen(xyz_end[0], xyz_end[1], xyz_end[2]); + xyz_result = (1 - alpha) * xyz_start_eigen + alpha * xyz_end_eigen; + } else if constexpr (Type == TrajectoryType::BEZIER) { + Eigen::Vector3d P0(xyz_start[0], xyz_start[1], xyz_start[2]); + Eigen::Vector3d P3(xyz_end[0], xyz_end[1], xyz_end[2]); + Eigen::Vector3d P1(xyz_control_1[0], xyz_control_1[1], xyz_control_1[2]); + Eigen::Vector3d P2(xyz_control_2[0], xyz_control_2[1], xyz_control_2[2]); + Eigen::Vector3d P01 = (1 - alpha) * P0 + alpha * P1; + Eigen::Vector3d P12 = (1 - alpha) * P1 + alpha * P2; + Eigen::Vector3d P23 = (1 - alpha) * P2 + alpha * P3; + Eigen::Vector3d P012 = (1 - alpha) * P01 + alpha * P12; + Eigen::Vector3d P123 = (1 - alpha) * P12 + alpha * P23; + xyz_result = (1 - alpha) * P012 + alpha * P123; + } + + Eigen::Vector3d rpy_start_eigen(rpy_start[0], rpy_start[1], rpy_start[2]); + Eigen::Vector3d rpy_end_eigen(rpy_end[0], rpy_end[1], rpy_end[2]); + Eigen::Vector3d rpy_result = (1 - alpha) * rpy_start_eigen + alpha * rpy_end_eigen; + + std::copy(xyz_result.data(), xyz_result.data() + 3, result.begin()); + std::copy(rpy_result.data(), rpy_result.data() + 3, result.begin() + 3); + } else if constexpr (Type == TrajectoryType::JOINT) { + // double a0 = joint_start[0] + Eigen::VectorXd result_(6), a0(6), a1(6), a2(6), a3(6), start(6), end(6); + start << joint_start[0], joint_start[1], joint_start[2], joint_start[3], + joint_start[4], joint_start[5]; + end << joint_end[0], joint_end[1], joint_end[2], joint_end[3], joint_end[4], + joint_end[5]; + + a0 = start; + a2 = 3.0f * (end - start) / pow(total_step, 2); + a3 = -2.0f * (end - start) / pow(total_step, 3); + result_ = a0 + a2 * pow(current_step, 2) + a3 * pow(current_step, 3); + std::copy(result_.data(), result_.data() + 6, result.begin()); + } + + current_step++; + } + + return result; + } + +private: + std::array result; + double total_step; + double current_step; + bool is_complete; + std::array xyz_start, xyz_end, xyz_control_1, xyz_control_2; + std::array rpy_start, rpy_end; +}; + +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/engineer.cpp b/rmcs_ws/src/rmcs_core/src/hardware/engineer.cpp new file mode 100644 index 00000000..867c19dd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/engineer.cpp @@ -0,0 +1,810 @@ +#include "hardware/device//encorder.hpp" +#include "hardware/device//joint.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dm_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/power_meter .hpp" +#include "hardware/device/relay.hpp" +#include "hardware/forwarder/cboard.hpp" +#include "hardware/ring_buffer.hpp" +#include "librmcs/device/bmi088.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::hardware { + +class Engineer + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Engineer() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , engineer_command_(create_partner_component("engineer_command", *this)) + , armboard_( + *this, *engineer_command_, + static_cast(get_parameter("arm_board_usb_pid").as_int())) + , leftboard_( + *this, *engineer_command_, + static_cast(get_parameter("left_board_usb_pid").as_int())) + , rightboard_( + *this, *engineer_command_, + static_cast(get_parameter("right_board_usb_pid").as_int())) {} + ~Engineer() override = default; + void update() override { + armboard_.update(); + leftboard_.update(); + rightboard_.update(); + } + void command() { + // armboard_.command(); + leftboard_.command(); + rightboard_.command(); + } + +private: + static double normalizeAngle(double angle) { + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + rclcpp::Logger logger_; + class EngineerCommand : public rmcs_executor::Component { + public: + explicit EngineerCommand(Engineer& engineer) + : engineer_(engineer) {} + void update() override { engineer_.command(); } + + Engineer& engineer_; + }; + std::shared_ptr engineer_command_; + + class ArmBoard final : private librmcs::client::CBoard, rclcpp::Node { + public: + friend class Engineer; + explicit ArmBoard(Engineer& engineer, EngineerCommand& engineer_command, int usb_pid) + : librmcs::client::CBoard(usb_pid) + , rclcpp::Node{"arm_board"} + + , joint( + {engineer, engineer_command, "/arm/Joint1"}, + {engineer, engineer_command, "/arm/Joint2"}, + {engineer, engineer_command, "/arm/Joint3"}, + {engineer, engineer_command, "/arm/Joint4"}, + {engineer, engineer_command, "/arm/Joint5"}, + {engineer, engineer_command, "/arm/Joint6"}) + , joint2_encoder(engineer, "/arm/Joint2encoder") + , joint3_encoder(engineer, "/arm/Joint3encoder") + , dr16_(engineer) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) + , bmi088_(1000, 0.2, 0) + , arm_pump{engineer, engineer_command, "/arm/pump"} + , mine_pump{engineer, engineer_command, "/mine/pump"} + , arm_pump_relay(engineer_command, "/arm/pump/relay", device::RelayType::One_CH) + + { + + engineer.register_output("/arm/Joint6/control_angle_error", joint6_error_angle); + engineer.register_output("/arm/Joint5/control_angle_error", joint5_error_angle); + engineer.register_output("/arm/Joint4/control_angle_error", joint4_error_angle); + engineer.register_output("/arm/Joint3/control_angle_error", joint3_error_angle); + engineer.register_output("/arm/Joint2/control_angle_error", joint2_error_angle); + engineer.register_output("/arm/Joint1/control_angle_error", joint1_error_angle); + + engineer.register_output("/arm/Joint1/vision", vision_theta1, NAN); + engineer.register_output("/arm/Joint2/vision", vision_theta2, NAN); + engineer.register_output("/arm/Joint3/vision", vision_theta3, NAN); + engineer.register_output("/arm/Joint4/vision", vision_theta4, NAN); + engineer.register_output("/arm/Joint5/vision", vision_theta5, NAN); + engineer.register_output("/arm/Joint6/vision", vision_theta6, NAN); + + engineer_command.register_input("/arm/enable_flag", is_arm_enable_, false); // + engineer_command.register_input("/arm/Joint1/zero_point", joint1_zero_point, NAN); // + engineer.register_output("yaw_imu_velocity", yaw_imu_velocity, NAN); + engineer.register_output("yaw_imu_angle", yaw_imu_angle, NAN); + + using namespace device; + joint[5].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i10V3}.set_encoder_zero_point( + static_cast(engineer.get_parameter("joint6_zero_point").as_int())), + DHConfig{0, -0.0571, 0, 0}, + Qlim_Stall_Config{engineer.get_parameter("joint6_qlim").as_double_array()}); + joint[4].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i10V3} + .enable_multi_turn_angle() + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast( + engineer.get_parameter("joint5_zero_point").as_int())), + DHConfig{0, 0, 1.5707963, 0}, + Qlim_Stall_Config{engineer.get_parameter("joint5_qlim").as_double_array()}); + joint[3].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i36V3}.set_encoder_zero_point( + static_cast(engineer.get_parameter("joint4_zero_point").as_int())), + DHConfig{0, 0.33969, 1.5707963, 0}, + Qlim_Stall_Config{engineer.get_parameter("joint4_qlim").as_double_array()}); + joint[2].configure_joint( + LKMotorConfig{LKMotorType::MF7015V210T}, DHConfig{-0.08307, 0, 1.5707963, 0}, + Qlim_Stall_Config{engineer.get_parameter("joint3_qlim").as_double_array()}); + joint[1].configure_joint( + LKMotorConfig{LKMotorType::MF7015V210T}, DHConfig{0.41, 0, 0, 1.5707963}, + Qlim_Stall_Config{engineer.get_parameter("joint2_qlim").as_double_array()}); + joint2_encoder.configure( + EncoderConfig{} + .set_encoder_zero_point( + static_cast(engineer.get_parameter("joint2_zero_point").as_int())) + .enable_multi_turn_angle()); + joint3_encoder.configure( + EncoderConfig{} + .set_encoder_zero_point( + static_cast(engineer.get_parameter("joint3_zero_point").as_int())) + .reverse()); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, -y, z); }); + arm_pump.configure(DjiMotorConfig{DjiMotorType::M3508}.set_reduction_ratio(1.0)); + mine_pump.configure(DjiMotorConfig{DjiMotorType::M3508}.set_reduction_ratio(1.0)); + } + ~ArmBoard() final { + stop_handling_events(); + event_thread_.join(); + } + + void update() { + using namespace device; + static std::vector qlim_input = {-3.1415926, 3.1415926}; + + joint[0].configure_joint( + device::LKMotorConfig{device::LKMotorType::MG8010E_i36}.set_encoder_zero_point( + (double)(*joint1_zero_point)), + DHConfig{0, 0.05985, 1.5707963, 0}, Qlim_Stall_Config{qlim_input}); + + update_arm_motors(); + dr16_.update(); + update_imu(); + } + void command() { arm_command_update(); } + + private: + void arm_command_update() { + auto is_arm_enable = *is_arm_enable_; + uint64_t command_; + uint16_t M3508_command[4]; + int max_count = 100000; + static int counter = 0; + + if (counter % 2 == 0) { + + *joint3_error_angle = + is_arm_enable + ? normalizeAngle(joint[2].get_target_theta() - joint[2].get_theta()) + : NAN; + command_ = joint[2].generate_torque_command(); + transmit_buffer_.add_can1_transmission( + 0x143, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + command_ = arm_pump_relay.generate_commande(); + transmit_buffer_.add_can2_transmission( + 0x305, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + M3508_command[0] = mine_pump.generate_command(); + M3508_command[1] = 0; + M3508_command[2] = 0; + M3508_command[3] = arm_pump.generate_command(); + ; + transmit_buffer_.add_can2_transmission( + 0x200, std::bit_cast(std::bit_cast(M3508_command))); + *joint6_error_angle = + is_arm_enable + ? normalizeAngle(joint[5].get_target_theta() - joint[5].get_theta()) + : NAN; + command_ = joint[5].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x146, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + } else { + *joint1_error_angle = + is_arm_enable ? (joint[0].get_target_theta() - joint[0].get_theta()) : NAN; + command_ = joint[0].generate_torque_command(); + transmit_buffer_.add_can1_transmission( + 0x141, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + *joint2_error_angle = + is_arm_enable + ? -normalizeAngle(joint[1].get_target_theta() - joint[1].get_theta()) + : NAN; + command_ = joint[1].generate_torque_command(); + transmit_buffer_.add_can1_transmission( + 0x142, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + *joint5_error_angle = + is_arm_enable + ? normalizeAngle(joint[4].get_target_theta() - joint[4].get_theta()) + : NAN; + command_ = joint[4].generate_torque_command(); + + transmit_buffer_.add_can2_transmission( + 0x145, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + *joint4_error_angle = + is_arm_enable + ? normalizeAngle(joint[3].get_target_theta() - joint[3].get_theta()) + : NAN; + command_ = joint[3].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x144, std::bit_cast(std::bit_cast(uint64_t{command_}))); + } + transmit_buffer_.trigger_transmission(); + last_is_arm_enable_ = is_arm_enable; + + counter++; + if (counter >= max_count) { + counter = 0; + } + } + + void update_arm_motors() { + joint2_encoder.update(); + joint3_encoder.update(); + + joint[5].update_joint(); + joint[4].update_joint(); + joint[3].update_joint(); + joint[2].update_joint().change_theta_feedback_(joint3_encoder.get_angle()); + joint[1].update_joint().change_theta_feedback_(joint2_encoder.get_angle()); + joint[0].update_joint(); + arm_pump.update(); + mine_pump.update(); + // RCLCPP_INFO(this->get_logger(), "joint %f", joint[1].get_theta()); + } + + void update_imu() { + bmi088_.update_status(); + + *yaw_imu_velocity = bmi088_.gz(); + *yaw_imu_angle = std::atan2( + 2.0 * (bmi088_.q0() * bmi088_.q3() + bmi088_.q1() * bmi088_.q2()), + 1.0 - 2.0 * (bmi088_.q2() * bmi088_.q2() + bmi088_.q3() * bmi088_.q3())); + } + + protected: + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x146) + joint[5].store_status(can_data); + if (can_id == 0x145) + joint[4].store_status(can_data); + if (can_id == 0x144) + joint[3].store_status(can_data); + if (can_id == 0x204) { + arm_pump.store_status(can_data); + } + if (can_id == 0x201) { + mine_pump.store_status(can_data); + } + } + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x143) + joint[2].store_status(can_data); + if (can_id == 0x142) + joint[1].store_status(can_data); + if (can_id == 0x141) + joint[0].store_status(can_data); + if (can_id == 0x005) + joint3_encoder.store_status(can_data); + if (can_id == 0x1fb) + joint2_encoder.store_status(can_data); + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + + } + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + bmi088_.store_gyroscope_status(x, y, z); + } + + private: + OutputInterface joint6_error_angle; + OutputInterface joint5_error_angle; + OutputInterface joint4_error_angle; + OutputInterface joint3_error_angle; + OutputInterface joint2_error_angle; + OutputInterface joint1_error_angle; + + OutputInterface vision_theta1; + OutputInterface vision_theta2; + OutputInterface vision_theta3; + OutputInterface vision_theta4; + OutputInterface vision_theta5; + OutputInterface vision_theta6; + OutputInterface> vision_data; + InputInterface is_arm_enable_; + InputInterface joint1_zero_point; + + device::Joint joint[6]; + device::Encoder joint2_encoder; + device::Encoder joint3_encoder; + device::Dr16 dr16_; + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + librmcs::utility::RingBuffer vision{39}; + bool last_is_arm_enable_ = true; + librmcs::device::Bmi088 bmi088_; + device::DjiMotor arm_pump, mine_pump; + device::Relay arm_pump_relay; + + OutputInterface yaw_imu_velocity; + OutputInterface yaw_imu_angle; + + } armboard_; + class LeftBoard final : private librmcs::client::CBoard, rclcpp::Node { + public: + friend class Engineer; + explicit LeftBoard(Engineer& engineer, EngineerCommand& engineer_command, int usb_pid) + : librmcs::client::CBoard(usb_pid) + , rclcpp::Node{"left_board"} + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) + , Steering_motors( + {engineer, engineer_command, "/steering/steering/lf"}, + {engineer, engineer_command, "/steering/steering/lb"}) + , Wheel_motors( + {engineer, engineer_command, "/steering/wheel/lf"}, + {engineer, engineer_command, "/steering/wheel/lb"}) + , power_meter(engineer, "/steering/power_meter") + , Omni_Motors(engineer, engineer_command, "/leg/omni/l") + , Leg_Motors( + {engineer, engineer_command, "/leg/joint/lf"}, + {engineer, engineer_command, "/leg/joint/lb"}) + , Leg_ecd({engineer, "/leg/encoder/lf"}, {engineer, "/leg/encoder/lb"}) { + Steering_motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::GM6020} + .reverse() + .set_encoder_zero_point( + static_cast( + engineer.get_parameter("steering_lf_zero_point").as_int()))); + Steering_motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::GM6020} + .reverse() + .set_encoder_zero_point( + static_cast( + engineer.get_parameter("steering_lb_zero_point").as_int()))); + + Wheel_motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(18.2)); + Wheel_motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(18.2)); + Leg_Motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508} + .set_reduction_ratio(92.0) + .reverse()); + Leg_Motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(277.6)); + Leg_ecd[0].configure( + device::EncoderConfig{}.set_encoder_zero_point( + static_cast(engineer.get_parameter("leg_lf_ecd_zero_point").as_int()))); + Leg_ecd[1].configure( + device::EncoderConfig{}.reverse().set_encoder_zero_point( + static_cast(engineer.get_parameter("leg_lb_ecd_zero_point").as_int()))); + Omni_Motors.configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.reverse().set_reduction_ratio( + 18.2)); + engineer.register_output( + "/leg/joint/lb/control_theta_error", leg_joint_lb_control_theta_error, NAN); + engineer.register_output( + "/leg/joint/lf/control_theta_error", leg_joint_lf_control_theta_error, NAN); + engineer_command.register_input("/leg/joint/lb/target_theta", leg_lb_target_theta_); + engineer_command.register_input("/leg/joint/lf/target_theta", leg_lf_target_theta_); + engineer_command.register_input("/leg/enable_flag", is_leg_enable_); + } + ~LeftBoard() final { + uint16_t command_[4] = {0, 0, 0, 0}; + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(command_)); + transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(command_)); + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(command_)); + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command_)); + transmit_buffer_.trigger_transmission(); + stop_handling_events(); + event_thread_.join(); + } + void update() { + Omni_Motors.update(); + for (auto& motor : Steering_motors) { + motor.update(); + } + for (auto& motor : Wheel_motors) { + motor.update(); + } + for (auto& motor : Leg_Motors) { + motor.update(); + } + for (auto& ecd : Leg_ecd) { + ecd.update(); + } + power_meter.update(); + // RCLCPP_INFO(this->get_logger(), "lf raw %d angle %f", + // Steering_motors[0].get_raw_angle(),Steering_motors[0].get_angle()); + // RCLCPP_INFO(this->get_logger(), "lb raw %d angle %f", + // Steering_motors[1].get_raw_angle(),Steering_motors[1].get_angle()); + } + void command() { + uint16_t command_[4]; + auto is_chassis_and_leg_enable = *is_leg_enable_; + + static int counter = 0; + if (counter % 2 == 0) { + command_[0] = 0; + command_[1] = 0; + command_[2] = Steering_motors[1].generate_command(); + command_[3] = 0; + transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(command_)); + command_[0] = Steering_motors[0].generate_command(); + command_[1] = 0; + command_[2] = 0; + command_[3] = 0; + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(command_)); + } else { + command_[0] = Wheel_motors[1].generate_command(); + if (*is_leg_enable_ == true) { + command_[1] = Leg_Motors[1].generate_command(); + } else { + command_[1] = 0; + } + + *leg_joint_lb_control_theta_error = + normalizeAngle(*leg_lb_target_theta_ - Leg_ecd[1].get_angle()); + command_[2] = 0; + command_[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(command_)); + command_[0] = Wheel_motors[0].generate_command(); + *leg_joint_lf_control_theta_error = + normalizeAngle(*leg_lf_target_theta_ - Leg_ecd[0].get_angle()); + if (*is_leg_enable_ == true) { + command_[1] = Leg_Motors[0].generate_command(); + } else { + command_[1] = 0; + } + command_[2] = Omni_Motors.generate_command(); + // command_[2]=0; + command_[3] = 0; + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command_)); + } + if (counter > 1000) { + counter = 0; + } + transmit_buffer_.trigger_transmission(); + counter++; + } + + protected: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + // RCLCPP_INFO(this->get_logger(), "leftcan1 %x", can_id); + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x205) { + Steering_motors[0].store_status(can_data); + } + if (can_id == 0x202) { + Leg_Motors[0].store_status(can_data); + } + if (can_id == 0x201) { + Wheel_motors[0].store_status(can_data); + } + if (can_id == 0x203) { + Omni_Motors.store_status(can_data); + } + if (can_id == 0x321) { + Leg_ecd[0].store_status(can_data); + } + } + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + // RCLCPP_INFO(this->get_logger(), "leftcan2 %x", can_id); + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x207) { + Steering_motors[1].store_status(can_data); + } + if (can_id == 0x201) { + Wheel_motors[1].store_status(can_data); + } + if (can_id == 0x202) { + Leg_Motors[1].store_status(can_data); + } + if (can_id == 0x100) { + power_meter.store_status(can_data); + } + if (can_id == 0x320) { + Leg_ecd[1].store_status(can_data); + } + } + + private: + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + device::DjiMotor Steering_motors[2]; + device::DjiMotor Wheel_motors[2]; + device::PowerMeter power_meter; + device::DjiMotor Omni_Motors; + device::DjiMotor Leg_Motors[2]; + InputInterface is_leg_enable_; + device::Encoder Leg_ecd[2]; + InputInterface leg_lf_target_theta_; + InputInterface leg_lb_target_theta_; + OutputInterface leg_joint_lb_control_theta_error; + OutputInterface leg_joint_lf_control_theta_error; + } leftboard_; + + class RightBoard final : private librmcs::client::CBoard, rclcpp::Node { + public: + friend class Engineer; + explicit RightBoard(Engineer& engineer, EngineerCommand& engineer_command, int usb_pid) + : librmcs::client::CBoard(usb_pid) + , rclcpp::Node{"right_board"} + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) + , Steering_motors( + {engineer, engineer_command, "/steering/steering/rb"}, + {engineer, engineer_command, "/steering/steering/rf"}) + , Wheel_motors( + {engineer, engineer_command, "/steering/wheel/rb"}, + {engineer, engineer_command, "/steering/wheel/rf"}) + , Omni_Motors(engineer, engineer_command, "/leg/omni/r") + , Leg_Motors( + {engineer, engineer_command, "/leg/joint/rb"}, + {engineer, engineer_command, "/leg/joint/rf"}) + , Leg_ecd({engineer, "/leg/encoder/rb"}, {engineer, "/leg/encoder/rf"}) + , big_yaw(engineer, engineer_command, "/chassis/big_yaw") { + Steering_motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::GM6020} + .reverse() + .set_encoder_zero_point( + static_cast( + engineer.get_parameter("steering_rb_zero_point").as_int()))); + Steering_motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::GM6020} + .reverse() + .set_encoder_zero_point( + static_cast( + engineer.get_parameter("steering_rf_zero_point").as_int()))); + Wheel_motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(18.2)); + Wheel_motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(18.2)); + Omni_Motors.configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(18.2)); + Leg_Motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.reverse().set_reduction_ratio( + 277.6)); + Leg_Motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(92.0)); + Leg_ecd[0].configure( + device::EncoderConfig{}.set_encoder_zero_point( + static_cast(engineer.get_parameter("leg_rb_ecd_zero_point").as_int()))); + Leg_ecd[1].configure( + device::EncoderConfig{}.reverse().set_encoder_zero_point( + static_cast(engineer.get_parameter("leg_rf_ecd_zero_point").as_int()))); + big_yaw.configure( + device::DMMotorConfig{device::DMMotorType::DM8009}.set_encoder_zero_point( + static_cast(engineer.get_parameter("big_yaw_zero_point").as_int()))); + engineer.register_output( + "/leg/joint/rb/control_theta_error", leg_joint_rb_control_theta_error, NAN); + engineer.register_output( + "/leg/joint/rf/control_theta_error", leg_joint_rf_control_theta_error, NAN); + + engineer_command.register_input("/leg/joint/rb/target_theta", leg_rb_target_theta_); + engineer_command.register_input("/leg/joint/rf/target_theta", leg_rf_target_theta_); + engineer_command.register_input("/leg/enable_flag", is_leg_enable_); + } + + ~RightBoard() final { + uint16_t command_[4] = {0, 0, 0, 0}; + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(command_)); + transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(command_)); + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(command_)); + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command_)); + transmit_buffer_.trigger_transmission(); + stop_handling_events(); + event_thread_.join(); + } + void update() { + // RCLCPP_INFO(this->get_logger(), + // "is_finished = %s", + // (*is_leg_enable_) ? "true" : "false"); + Omni_Motors.update(); + for (auto& motor : Steering_motors) { + motor.update(); + } + for (auto& motor : Wheel_motors) { + motor.update(); + } + for (auto& motor : Leg_Motors) { + motor.update(); + } + for (auto& ecd : Leg_ecd) { + ecd.update(); + } + big_yaw.update(); + // RCLCPP_INFO(this->get_logger(), "rb raw %d angle %f", + // Steering_motors[0].get_raw_angle(),Steering_motors[0].get_angle()); + // RCLCPP_INFO(this->get_logger(), "rf raw %d angle %f", + // Steering_motors[1].get_raw_angle(),Steering_motors[1].get_angle()); + } + + void command() { + uint16_t command_[4]; + auto is_chassis_and_leg_enable = *is_leg_enable_; + + static int counter = 0; + if (counter % 2 == 0) { + command_[0] = 0; + command_[1] = 0; + command_[2] = 0; + command_[3] = Steering_motors[0].generate_command(); + transmit_buffer_.add_can2_transmission(0x1FE, std::bit_cast(command_)); + command_[0] = 0; + command_[1] = Steering_motors[1].generate_command(); + command_[2] = 0; + command_[3] = 0; + transmit_buffer_.add_can1_transmission(0x1FE, std::bit_cast(command_)); + } else { + command_[0] = Wheel_motors[0].generate_command(); + if (*is_leg_enable_ == true) { + command_[1] = Leg_Motors[0].generate_command(); + } else { + command_[1] = 0; + } + *leg_joint_rb_control_theta_error = + normalizeAngle(*leg_rb_target_theta_ - Leg_ecd[0].get_angle()); + command_[2] = 0; + command_[3] = 0; + transmit_buffer_.add_can2_transmission(0x200, std::bit_cast(command_)); + command_[0] = Wheel_motors[1].generate_command(); + *leg_joint_rf_control_theta_error = + normalizeAngle(*leg_rf_target_theta_ - Leg_ecd[1].get_angle()); + if (*is_leg_enable_ == true) { + command_[1] = Leg_Motors[1].generate_command(); + } else { + command_[1] = 0; + } + command_[2] = Omni_Motors.generate_command(); + // command_[2]=0; + command_[3] = 0; + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command_)); + } + if (counter > 1000) { + counter = 0; + } + uint64_t command_1; + if (counter % 2 == 0) { + // if (is_chassis_and_leg_enable && big_yaw.get_state() != 0 + // && big_yaw.get_state() != 1) { + // command_1 = big_yaw.dm_clear_error_command(); + // } else if (!is_chassis_and_leg_enable) { + // command_1 = big_yaw.dm_close_command(); + // } else if (is_chassis_and_leg_enable && big_yaw.get_state() == 0) { + // command_1 = big_yaw.dm_enable_command(); + + // } else { + // command_1 = big_yaw.generate_torque_command(); + // } + command_1 = big_yaw.dm_close_command(); + transmit_buffer_.add_can1_transmission( + (0x3), std::bit_cast(std::bit_cast(uint64_t{command_1}))); + } + transmit_buffer_.trigger_transmission(); + counter++; + } + + protected: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + + // RCLCPP_INFO(this->get_logger(), "rightcan1 %x", can_id); + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x201) { + + Wheel_motors[1].store_status(can_data); + } + if (can_id == 0x202) { + Leg_Motors[1].store_status(can_data); + } + if (can_id == 0x203) { + Omni_Motors.store_status(can_data); + } + if (can_id == 0x206) { + Steering_motors[1].store_status(can_data); + } + if (can_id == 0x322) { + Leg_ecd[1].store_status(can_data); + } + } + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + // RCLCPP_INFO(this->get_logger(), "rightcan2 %x", can_id); + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] { + // RCLCPP_INFO(this->get_logger(), "%x", can_id); + return; + } + + if (can_id == 0x201) { + // RCLCPP_INFO(this->get_logger(), "w0 %x", can_id); + Wheel_motors[0].store_status(can_data); + } + if (can_id == 0x202) { + Leg_Motors[0].store_status(can_data); + } + if (can_id == 0x208) { + Steering_motors[0].store_status(can_data); + } + if (can_id == 0x33) { + big_yaw.store_status(can_data); + } + if (can_id == 0x319) { + Leg_ecd[0].store_status(can_data); + } + } + + private: + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + device::DjiMotor Steering_motors[2]; + device::DjiMotor Wheel_motors[2]; + device::DjiMotor Omni_Motors; + device::DjiMotor Leg_Motors[2]; + device::Encoder Leg_ecd[2]; + device::DMMotor big_yaw; + + InputInterface leg_rf_target_theta_; + InputInterface leg_rb_target_theta_; + InputInterface is_leg_enable_; + + OutputInterface leg_joint_rb_control_theta_error; + + OutputInterface leg_joint_rf_control_theta_error; + + double max_lf = 0.0, max_lb = 0.0, max_rb = 0.0, max_rf = 0.0; + } rightboard_; +}; + +} // namespace rmcs_core::hardware +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Engineer, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM.hpp new file mode 100644 index 00000000..cacb90b4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#define up 1 +#define down 2 +#define initial_enter 3 + +template +class FiniteStateMachine; + +template +class IState { +public: + virtual ~IState() = default; + virtual void enter(FiniteStateMachine&, const C&) {} + virtual void exit(FiniteStateMachine&, const C&) {} + virtual std::shared_ptr> handleEvent(FiniteStateMachine&, const E&, C&) = 0; + virtual S getStateID() const = 0; +}; + +template +struct Transition { + std::function condition; + std::function action; + std::shared_ptr> target; +}; + +template +class FiniteStateMachine { + using StatePtr = std::shared_ptr>; + + std::unordered_map states_; + std::unordered_map>>> transitions_; + StatePtr currentState_; + C context_; + mutable std::mutex mtx_; + +public: + explicit FiniteStateMachine(C context = C{}) : context_(context) {} + + template + void registerState(Args&&... args) { + std::lock_guard lock(mtx_); + auto state = std::make_shared(std::forward(args)...); + states_.emplace(state->getStateID(), state); + } + + std::shared_ptr> getState(S stateID) const { + std::lock_guard lock(mtx_); + auto it = states_.find(stateID); + if (it != states_.end()) { + return it->second; + } + return nullptr; + } + + template + void addTransition(S from, EventType&& event, + std::function condition, + std::function action, + S to) { + std::lock_guard lock(mtx_); + auto& eventTransitions = transitions_[typeid(EventType)][from]; + eventTransitions.emplace_back(Transition{ + [condition=std::move(condition)](const E& e, const C& c) { + if constexpr (std::is_same_v) { + return condition(static_cast(e), c); + } + return false; + }, + [action=std::move(action)](const E& e, C& c) { + if constexpr (std::is_same_v) { + action(static_cast(e), c); + } + }, + states_.at(to) + }); + } + + void start(S initialState) { + std::lock_guard lock(mtx_); + currentState_ = states_.at(initialState); + currentState_->enter(*this, context_); + } + + void processEvent(const E& event) { + std::lock_guard lock(mtx_); + if (!currentState_) return; + + auto& typeTransitions = transitions_[typeid(event)]; + auto stateTransitions = typeTransitions.find(currentState_->getStateID()); + if (stateTransitions == typeTransitions.end()) return; + + for (auto& transition : stateTransitions->second) { + if (transition.condition(event, context_)) { + currentState_->exit(*this, context_); + transition.action(event, context_); + currentState_ = transition.target; + currentState_->enter(*this, context_); + break; + } + } + } + + + S getCurrentState() const { + std::lock_guard lock(mtx_); + return currentState_ ? currentState_->getStateID() : S{}; + } + + C& getContext() { return context_; } + const C& getContext() const { return context_; } +}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_l.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_l.hpp new file mode 100644 index 00000000..f7d8f7ad --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_l.hpp @@ -0,0 +1,163 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include + +enum class Auto_Gold_State { Set_initial, Lift }; +enum class Auto_Gold_Event { Up, Down }; +// 定义上下文 +struct Auto_Gold_Context {}; + +class Gold_Set_initial_State : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Gold_Context& context) override { + // RCLCPP_INFO(logger, "bbbb"); + } + void exit( + FiniteStateMachine& fsm, + const Auto_Gold_Context& context) override { + // RCLCPP_INFO(logger, "cccc"); + } + std::shared_ptr> handleEvent( + FiniteStateMachine& fsm, + const Auto_Gold_Event& event, Auto_Gold_Context& context) override { + if (event == Auto_Gold_Event::Up) { + return fsm.getState(Auto_Gold_State::Lift); // 转到Lift状态 + } + return nullptr; + } + Auto_Gold_State getStateID() const override { return Auto_Gold_State::Set_initial; } + +private: + rclcpp::Logger logger = rclcpp::get_logger("Set_initial_State"); +}; +class Gold_Lift_State : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Gold_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Gold_Context& context) override {} + std::shared_ptr> handleEvent( + FiniteStateMachine& fsm, + const Auto_Gold_Event& event, Auto_Gold_Context& context) override { + + if (event == Auto_Gold_Event::Down) { + return fsm.getState(Auto_Gold_State::Set_initial); + } + return nullptr; + } + Auto_Gold_State getStateID() const override { return Auto_Gold_State::Lift; } + +private: + rclcpp::Logger logger = rclcpp::get_logger("Lift_State"); +}; +class Auto_Gold_Left { +public: + explicit Auto_Gold_Left() { + std::array lift_start_point_position = {0.11, -0.65, 0.14}; + std::array lift_end_point_position = {0.11, -0.65, 0.180}; + std::array lift_point_orientation = { + -std::numbers::pi / 2.0, -14.0 * std::numbers::pi / 180.0, -std::numbers::pi / 2.0}; + + std::array initial_joint_theta = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_start_point_position[0], lift_start_point_position[1], + lift_start_point_position[2], lift_point_orientation[0], lift_point_orientation[1], + lift_point_orientation[2]}); + + initial_joint_theta[3] = std::numbers::pi; + initial_joint_theta[5] = 0; + reset_initial_arm.set_total_step(2000).set_end_point(initial_joint_theta); + lift_mine.set_total_step(300) + .set_start_point(lift_start_point_position, lift_point_orientation) + .set_end_point(lift_end_point_position, lift_point_orientation); + + fsm.registerState(); + fsm.registerState(); + fsm.addTransition( + Auto_Gold_State::Set_initial, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + if (reset_initial_arm.get_complete()) { + return true; + } + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Lift); + + fsm.addTransition( + Auto_Gold_State::Lift, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + std::array result_ = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + lift_mine.trajectory()); + result_[3] = std::numbers::pi; + result_[5] = 0; + result = result_; + reset_initial_arm.reset(); + } + if (event == Auto_Gold_Event::Down) { + if (lift_mine.get_complete()) { + reset_initial_arm.set_total_step(500); + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + if (reset_initial_arm.get_complete()) { + reset_initial_arm.reset(); + lift_mine.reset(); + } + } + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Set_initial); + + + fsm.start(Auto_Gold_State::Set_initial); + } + Auto_Gold_State getState() { return fsm.getCurrentState(); } + void get_current_theta(std::array theta) { last_theta_ = theta; } + std::array get_result() { return result; } + bool get_first_trajectory_result() { return reset_initial_arm.get_complete(); } + void reset_first_trajectory() { reset_initial_arm.reset(); } + bool get_second_trajectory_result() { return lift_mine.get_complete(); } + + void processEvent(const Auto_Gold_Event& event) { fsm.processEvent(event); } + + void reset() { + fsm.start(Auto_Gold_State::Set_initial); + fsm_direction = initial_enter; + + reset_initial_arm.reset(); + lift_mine.reset(); + } + int fsm_direction = initial_enter; + +private: + Auto_Gold_Context context_; + FiniteStateMachine fsm{context_}; + std::array last_theta_, result; + rclcpp::Logger logger = rclcpp::get_logger("Auto_gold"); + rmcs_core::hardware::device::Trajectory + reset_initial_arm; + + rmcs_core::hardware::device::Trajectory + lift_mine; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_m.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_m.hpp new file mode 100644 index 00000000..051b7e24 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_m.hpp @@ -0,0 +1,122 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include + +#include "hardware/fsm/FSM_gold_l.hpp" +class Auto_Gold_Mid { +public: + explicit Auto_Gold_Mid() { + std::array lift_start_point_position = {0.66, 0.0, 0.145}; + std::array lift_end_point_position = {0.66, 0.0, 0.1850}; + std::array lift_point_orientation = { + 0.0, -std::numbers::pi/2.0, 0.0}; + + std::array initial_joint_theta = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_start_point_position[0], lift_start_point_position[1], + lift_start_point_position[2], lift_point_orientation[0], lift_point_orientation[1], + lift_point_orientation[2]}); + + initial_joint_theta[3] = std::numbers::pi; + initial_joint_theta[5] = 0; + reset_initial_arm.set_total_step(2000).set_end_point(initial_joint_theta); + lift_mine.set_total_step(300) + .set_start_point(lift_start_point_position, lift_point_orientation) + .set_end_point(lift_end_point_position, lift_point_orientation); + + fsm.registerState(); + fsm.registerState(); + fsm.addTransition( + Auto_Gold_State::Set_initial, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + if (reset_initial_arm.get_complete()) { + return true; + } + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Lift); + + fsm.addTransition( + Auto_Gold_State::Lift, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + std::array result_ = rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + lift_mine.trajectory()); + result_[3] = std::numbers::pi; + result_[5] = 0; + result = result_; + reset_initial_arm.reset(); + } + if (event == Auto_Gold_Event::Down) { + if (lift_mine.get_complete()) { + reset_initial_arm.set_total_step(500); + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + if (reset_initial_arm.get_complete()) { + reset_initial_arm.reset(); + lift_mine.reset(); + } + } + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Set_initial); + + // fsm.addTransition( + // Auto_Gold_State::Lift, Auto_Gold_Event::Down, + // [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + + // RCLCPP_INFO(logger, "e"); + + // return false; + // }, + // [this]( + // const Auto_Gold_Event& event, + // Auto_Gold_Context& context) { }, + // Auto_Gold_State::Set_initial); + fsm.start(Auto_Gold_State::Set_initial); + } + Auto_Gold_State getState() { return fsm.getCurrentState(); } + void get_current_theta(std::array theta) { last_theta_ = theta; } + std::array get_result() { return result; } + bool get_first_trajectory_result() { return reset_initial_arm.get_complete(); } + void reset_first_trajectory() { reset_initial_arm.reset(); } + bool get_second_trajectory_result() { return lift_mine.get_complete(); } + + void processEvent(const Auto_Gold_Event& event) { fsm.processEvent(event); } + + void reset() { + fsm.start(Auto_Gold_State::Set_initial); + fsm_direction = initial_enter; + + reset_initial_arm.reset(); + lift_mine.reset(); + } + int fsm_direction = initial_enter; + +private: + Auto_Gold_Context context_; + FiniteStateMachine fsm{context_}; + std::array last_theta_, result; + rclcpp::Logger logger = rclcpp::get_logger("Auto_gold"); + rmcs_core::hardware::device::Trajectory + reset_initial_arm; + + rmcs_core::hardware::device::Trajectory + lift_mine; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_r.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_r.hpp new file mode 100644 index 00000000..18a2d959 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_gold_r.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include + +#include "hardware/fsm/FSM_gold_l.hpp" +class Auto_Gold_Right { +public: + explicit Auto_Gold_Right() { + std::array lift_start_point_position = {0.11, 0.65, 0.14}; + std::array lift_end_point_position = {0.11, 0.65, 0.180}; + std::array lift_point_orientation = { + -std::numbers::pi / 2.0, -14.0 * std::numbers::pi / 180.0, std::numbers::pi / 2.0}; + + std::array initial_joint_theta = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_start_point_position[0], lift_start_point_position[1], + lift_start_point_position[2], lift_point_orientation[0], lift_point_orientation[1], + lift_point_orientation[2]}); + + initial_joint_theta[3] = std::numbers::pi; + initial_joint_theta[5] = 0; + reset_initial_arm.set_total_step(2000).set_end_point(initial_joint_theta); + lift_mine.set_total_step(600) + .set_start_point(lift_start_point_position, lift_point_orientation) + .set_end_point(lift_end_point_position, lift_point_orientation); + fsm.registerState(); + fsm.registerState(); + fsm.addTransition( + Auto_Gold_State::Set_initial, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + if (reset_initial_arm.get_complete()) { + return true; + } + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Lift); + + fsm.addTransition( + Auto_Gold_State::Lift, Auto_Gold_Event::Up, + [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + if (event == Auto_Gold_Event::Up) { + std::array result_ = rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + lift_mine.trajectory()); + result_[3] = std::numbers::pi; + result_[5] = 0; + result = result_; + reset_initial_arm.reset(); + } + if (event == Auto_Gold_Event::Down) { + if (lift_mine.get_complete()) { + reset_initial_arm.set_total_step(500); + reset_initial_arm.set_start_point(last_theta_); + result = reset_initial_arm.trajectory(); + if (reset_initial_arm.get_complete()) { + reset_initial_arm.reset(); + lift_mine.reset(); + } + } + } + return false; + }, + [this](const Auto_Gold_Event& event, Auto_Gold_Context& context) {}, + Auto_Gold_State::Set_initial); + + // fsm.addTransition( + // Auto_Gold_State::Lift, Auto_Gold_Event::Down, + // [this](const Auto_Gold_Event& event, const Auto_Gold_Context& context) { + + // RCLCPP_INFO(logger, "e"); + + // return false; + // }, + // [this]( + // const Auto_Gold_Event& event, + // Auto_Gold_Context& context) { }, + // Auto_Gold_State::Set_initial); + fsm.start(Auto_Gold_State::Set_initial); + } + Auto_Gold_State getState() { return fsm.getCurrentState(); } + void get_current_theta(std::array theta) { last_theta_ = theta; } + std::array get_result() { return result; } + bool get_first_trajectory_result() { return reset_initial_arm.get_complete(); } + void reset_first_trajectory() { reset_initial_arm.reset(); } + bool get_second_trajectory_result() { return lift_mine.get_complete(); } + + void processEvent(const Auto_Gold_Event& event) { fsm.processEvent(event); } + + void reset() { + fsm.start(Auto_Gold_State::Set_initial); + fsm_direction = initial_enter; + + reset_initial_arm.reset(); + lift_mine.reset(); + } + int fsm_direction = initial_enter; + +private: + Auto_Gold_Context context_; + FiniteStateMachine fsm{context_}; + std::array last_theta_, result; + rclcpp::Logger logger = rclcpp::get_logger("Auto_gold"); + rmcs_core::hardware::device::Trajectory + reset_initial_arm; + + rmcs_core::hardware::device::Trajectory + lift_mine; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_sliver.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_sliver.hpp new file mode 100644 index 00000000..3c7edebe --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_sliver.hpp @@ -0,0 +1,158 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +enum class Auto_Sliver_State { + Set_initial, + Lift_mine, +}; +enum class Auto_Sliver_Event { Up, Down }; +struct Auto_Sliver_Context {}; +class Sliver_Set_initial_State + : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Sliver_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Sliver_Context& context) override {} + std::shared_ptr> handleEvent( + FiniteStateMachine& fsm, + const Auto_Sliver_Event& event, Auto_Sliver_Context& context) override { + + if (event == Auto_Sliver_Event::Up) { + return fsm.getState(Auto_Sliver_State::Lift_mine); + } + return nullptr; + } + Auto_Sliver_State getStateID() const override { return Auto_Sliver_State::Set_initial; } +}; + +class Sliver_Lift_mine_State + : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Sliver_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Sliver_Context& context) override {} + std::shared_ptr> handleEvent( + FiniteStateMachine& fsm, + const Auto_Sliver_Event& event, Auto_Sliver_Context& context) override { + + if (event == Auto_Sliver_Event::Down) { + return fsm.getState(Auto_Sliver_State::Set_initial); + } + return nullptr; + } + Auto_Sliver_State getStateID() const override { return Auto_Sliver_State::Lift_mine; } +}; + +class Auto_Sliver +// rclcpp::Node +{ +public: + explicit Auto_Sliver() + // : rclcpp::Node{"adada"} + { + std::array lift_start_point_position = {0.37, 0.001, -0.07}; + std::array lift_end_point_position = {0.37, 0.001, 0.26}; + + std::array lift_point_orientation = {0.0, 0.0, 0.0}; + + std::array initial_joint_theta = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_start_point_position[0], lift_start_point_position[1], + lift_start_point_position[2], lift_point_orientation[0], lift_point_orientation[1], + lift_point_orientation[2]}); + reset_initial_arm.set_total_step(2200).set_end_point(initial_joint_theta); + lift_mine.set_total_step(500) + .set_start_point(lift_start_point_position, lift_point_orientation) + .set_end_point(lift_end_point_position, lift_point_orientation); + + fsm.registerState(); + fsm.registerState(); + fsm.addTransition( + Auto_Sliver_State::Set_initial, Auto_Sliver_Event::Up, + [this](const Auto_Sliver_Event& event, const Auto_Sliver_Context& context) { + if (event == Auto_Sliver_Event::Up) { + if (reset_initial_arm.get_complete()) { + return true; + } + reset_initial_arm.set_start_point(enter_theta_); + result = reset_initial_arm.trajectory(); + } + return false; + }, + [this](const Auto_Sliver_Event& event, Auto_Sliver_Context& context) {}, + Auto_Sliver_State::Lift_mine); + + fsm.addTransition( + Auto_Sliver_State::Lift_mine, Auto_Sliver_Event::Up, + [this](const Auto_Sliver_Event& event, const Auto_Sliver_Context& context) { + if (event == Auto_Sliver_Event::Up) { + std::array result_ = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + lift_mine.trajectory()); + result_[3] = 0.0; + result_[5] = 0.0; + result = result_; + reset_initial_arm.reset(); + } + if (event == Auto_Sliver_Event::Down) { + if (lift_mine.get_complete()) { + reset_initial_arm.set_total_step(500); + reset_initial_arm.set_start_point(enter_theta_); + result = reset_initial_arm.trajectory(); + if (reset_initial_arm.get_complete()) { + reset_initial_arm.reset(); + lift_mine.reset(); + } + } + } + return false; + }, + [this](const Auto_Sliver_Event& event, Auto_Sliver_Context& context) {}, + Auto_Sliver_State::Set_initial); + fsm.start(Auto_Sliver_State::Set_initial); + } + Auto_Sliver_State getState() { return fsm.getCurrentState(); } + void get_current_theta(std::array theta) { enter_theta_ = theta; } + std::array get_result() { return result; } + bool get_first_trajectory_result() { return reset_initial_arm.get_complete(); } + void reset_first_trajectory() { reset_initial_arm.reset(); } + bool get_second_trajectory_result() { return lift_mine.get_complete(); } + + void processEvent(const Auto_Sliver_Event& event) { fsm.processEvent(event); } + + void reset() { + fsm.start(Auto_Sliver_State::Set_initial); + fsm_direction = initial_enter; + reset_initial_arm.reset(); + lift_mine.reset(); + } + int fsm_direction = initial_enter; + +private: + Auto_Sliver_Context context_; + FiniteStateMachine fsm{context_}; + std::array enter_theta_, result; + rmcs_core::hardware::device::Trajectory + reset_initial_arm; + + rmcs_core::hardware::device::Trajectory + lift_mine; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_lb.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_lb.hpp new file mode 100644 index 00000000..780c2b09 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_lb.hpp @@ -0,0 +1,156 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include +#include + +enum class Auto_Storage_State { + Set_Storage, +}; +enum class Auto_Storage_Event { Up, Down }; +struct Auto_Storage_Context {}; + +class Set_Storage_State + : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Storage_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Storage_Context& context) override {} + std::shared_ptr> + handleEvent( + FiniteStateMachine& fsm, + const Auto_Storage_Event& event, Auto_Storage_Context& context) override { + + if (event == Auto_Storage_Event::Up) { + return fsm.getState(Auto_Storage_State::Set_Storage); + } + return nullptr; + } + Auto_Storage_State getStateID() const override { return Auto_Storage_State::Set_Storage; } +}; + +class Auto_Storage_LB { +public: + explicit Auto_Storage_LB() { + + move_to_former_target.set_total_step(600); + move_to_target.set_total_step(900); + press.set_total_step(600) + .set_start_point(press_start_point_position, press_point_orientation) + .set_end_point(press_end_point_position, press_point_orientation); + delay.set_total_step(1100) + .set_start_point({0, 0, 0, 0, 0, 0}) + .set_end_point({0, 0, 0, 0, 0, 0}); + lift.set_total_step(600) + .set_start_point(lift_start_point_position, press_point_orientation) + .set_end_point(lift_end_point_position, press_point_orientation); + back_to_safety_.set_total_step(300) + .set_start_point(back_to_safety) + .set_end_point( + {0.0, back_to_safety[1], back_to_safety[2], back_to_safety[3], back_to_safety[4], + back_to_safety[5]}); + fsm.registerState(); + fsm.addTransition( + Auto_Storage_State::Set_Storage, Auto_Storage_Event::Up, + [this](const Auto_Storage_Event& event, const Auto_Storage_Context& context) { + if (event == Auto_Storage_Event::Up) { + move_to_former_target.set_start_point(last_theta_); + move_to_former_target.set_end_point( + {last_theta_[0], move_to_target_[1], move_to_target_[2], move_to_target_[3], + move_to_target_[4], move_to_target_[5]}); + move_to_target + .set_start_point( + {last_theta_[0], move_to_target_[1], move_to_target_[2], + move_to_target_[3], move_to_target_[4], move_to_target_[5]}) + .set_end_point(move_to_target_); + if (!move_to_former_target.get_complete()) { + result = move_to_former_target.trajectory(); + } else { + if (!move_to_target.get_complete()) { + result = move_to_target.trajectory(); + } else { + if (!press.get_complete()) { + result = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {press.trajectory()}); + } else { + if (!delay.get_complete()) { + delay.trajectory(); + } else { + if (!lift.get_complete()) { + result = rmcs_core::hardware::device::Kinematic:: + arm_inverse_kinematic(lift.trajectory()); + } else { + result = back_to_safety_.trajectory(); + } + } + } + } + } + } + return false; + }, + [this](const Auto_Storage_Event& event, Auto_Storage_Context& context) {}, + Auto_Storage_State::Set_Storage); + fsm.start(Auto_Storage_State::Set_Storage); + } + void get_current_theta(std::array theta) { last_theta_ = theta; } + std::array get_result() { return result; } + auto getState() { return fsm.getCurrentState(); } + void processEvent(const Auto_Storage_Event& event) { fsm.processEvent(event); } + bool get_first_trajectory_result() { return move_to_target.get_complete(); } + + int fsm_direction = initial_enter; + void reset() { + fsm.start(Auto_Storage_State::Set_Storage); + move_to_target.reset(); + press.reset(); + delay.reset(); + move_to_former_target.reset(); + lift.reset(); + back_to_safety_.reset(); + fsm_direction = initial_enter; + } + +private: + Auto_Storage_Context context_; + FiniteStateMachine fsm{context_}; + rmcs_core::hardware::device::Trajectory + move_to_target; + rmcs_core::hardware::device::Trajectory + move_to_former_target; + rmcs_core::hardware::device::Trajectory + press; + rmcs_core::hardware::device::Trajectory + delay; + rmcs_core::hardware::device::Trajectory lift; + rmcs_core::hardware::device::Trajectory + back_to_safety_; + std::array last_theta_, result; + std::array press_start_point_position = {-0.22, 0.24, 0.35}; + std::array press_end_point_position = {-0.22, 0.24, 0.30}; + std::array lift_start_point_position = {-0.22, 0.24, 0.30}; + std::array lift_end_point_position = {-0.22, 0.24, 0.38}; + std::array press_point_orientation = {-91 * std::numbers::pi / 180.0, 0.0, 0.0}; + + std::array move_to_target_ = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {press_start_point_position[0], press_start_point_position[1], + press_start_point_position[2], press_point_orientation[0], press_point_orientation[1], + press_point_orientation[2]}); + std::array back_to_safety = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_end_point_position[0], lift_end_point_position[1], lift_end_point_position[2], + press_point_orientation[0], press_point_orientation[1], press_point_orientation[2]}); +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_rb.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_rb.hpp new file mode 100644 index 00000000..3f3ee800 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_storage_rb.hpp @@ -0,0 +1,130 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include +#include +#include "hardware/fsm/FSM_storage_lb.hpp" + + +class Auto_Storage_RB { +public: + explicit Auto_Storage_RB() { + + move_to_former_target.set_total_step(600); + move_to_target.set_total_step(900); + press.set_total_step(600) + .set_start_point(press_start_point_position, press_point_orientation) + .set_end_point(press_end_point_position, press_point_orientation); + delay.set_total_step(1500) + .set_start_point({0, 0, 0, 0, 0, 0}) + .set_end_point({0, 0, 0, 0, 0, 0}); + lift.set_total_step(900) + .set_start_point(lift_start_point_position, press_point_orientation) + .set_end_point(lift_end_point_position, press_point_orientation); + back_to_safety_.set_total_step(300) + .set_start_point(back_to_safety) + .set_end_point( + {0.0, back_to_safety[1], back_to_safety[2], back_to_safety[3], back_to_safety[4], + back_to_safety[5]}); + fsm.registerState(); + fsm.addTransition( + Auto_Storage_State::Set_Storage, Auto_Storage_Event::Up, + [this](const Auto_Storage_Event& event, const Auto_Storage_Context& context) { + if (event == Auto_Storage_Event::Up) { + move_to_former_target.set_start_point(last_theta_); + move_to_former_target.set_end_point( + {last_theta_[0], move_to_target_[1], move_to_target_[2], move_to_target_[3], + move_to_target_[4], move_to_target_[5]}); + move_to_target + .set_start_point( + {last_theta_[0], move_to_target_[1], move_to_target_[2], + move_to_target_[3], move_to_target_[4], move_to_target_[5]}) + .set_end_point(move_to_target_); + if (!move_to_former_target.get_complete()) { + result = move_to_former_target.trajectory(); + } else { + if (!move_to_target.get_complete()) { + result = move_to_target.trajectory(); + } else { + if (!press.get_complete()) { + result = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {press.trajectory()}); + } else { + if (!delay.get_complete()) { + delay.trajectory(); + } else { + if (!lift.get_complete()) { + result = rmcs_core::hardware::device::Kinematic:: + arm_inverse_kinematic(lift.trajectory()); + } else { + result = back_to_safety_.trajectory(); + } + } + } + } + } + } + return false; + }, + [this](const Auto_Storage_Event& event, Auto_Storage_Context& context) {}, + Auto_Storage_State::Set_Storage); + fsm.start(Auto_Storage_State::Set_Storage); + } + void get_current_theta(std::array theta) { last_theta_ = theta; } + std::array get_result() { return result; } + auto getState() { return fsm.getCurrentState(); } + void processEvent(const Auto_Storage_Event& event) { fsm.processEvent(event); } + bool get_first_trajectory_result() { return move_to_target.get_complete(); } + + int fsm_direction = initial_enter; + void reset() { + fsm.start(Auto_Storage_State::Set_Storage); + move_to_target.reset(); + press.reset(); + delay.reset(); + move_to_former_target.reset(); + lift.reset(); + back_to_safety_.reset(); + fsm_direction = initial_enter; + } + +private: + Auto_Storage_Context context_; + FiniteStateMachine fsm{context_}; + rmcs_core::hardware::device::Trajectory + move_to_target; + rmcs_core::hardware::device::Trajectory + move_to_former_target; + rmcs_core::hardware::device::Trajectory + press; + rmcs_core::hardware::device::Trajectory + delay; + rmcs_core::hardware::device::Trajectory lift; + rmcs_core::hardware::device::Trajectory + back_to_safety_; + std::array last_theta_, result; + std::array press_start_point_position = {-0.23, -0.20, 0.36}; + std::array press_end_point_position = {-0.23, -0.20, 0.31}; + std::array lift_start_point_position = {-0.23, -0.20, 0.31}; + std::array lift_end_point_position = {-0.23, -0.20, 0.36}; + std::array press_point_orientation = {69 * std::numbers::pi / 180.0, 0.0, 0.0}; + + std::array move_to_target_ = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {press_start_point_position[0], press_start_point_position[1], + press_start_point_position[2], press_point_orientation[0], press_point_orientation[1], + press_point_orientation[2]}); + std::array back_to_safety = + rmcs_core::hardware::device::Kinematic::arm_inverse_kinematic( + {lift_end_point_position[0], lift_end_point_position[1], lift_end_point_position[2], + press_point_orientation[0], press_point_orientation[1], press_point_orientation[2]}); +}; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_up_stairs.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_up_stairs.hpp new file mode 100644 index 00000000..08cf4937 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_up_stairs.hpp @@ -0,0 +1,133 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include +#include +enum class Auto_Up_Stairs_State { Leg_Initial, Leg_Lift }; +enum class Auto_Up_Stairs_Event { Up, Down }; +struct Auto_Up_Stairs_Context {}; +class Leg_Initial_State + : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Up_Stairs_Context& context) override { + // RCLCPP_INFO(logger, "bbbb"); + } + void exit( + FiniteStateMachine& fsm, + const Auto_Up_Stairs_Context& context) override { + // RCLCPP_INFO(logger, "cccc"); + } + std::shared_ptr> + handleEvent( + FiniteStateMachine& + fsm, + const Auto_Up_Stairs_Event& event, Auto_Up_Stairs_Context& context) override { + if (event == Auto_Up_Stairs_Event::Up) { + return fsm.getState(Auto_Up_Stairs_State::Leg_Lift); + } + return nullptr; + } + Auto_Up_Stairs_State getStateID() const override { return Auto_Up_Stairs_State::Leg_Initial; } + +private: + rclcpp::Logger logger = rclcpp::get_logger("Set_initial_State"); +}; +class Leg_Press_State + : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Up_Stairs_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Up_Stairs_Context& context) override {} + std::shared_ptr> + handleEvent( + FiniteStateMachine& + fsm, + const Auto_Up_Stairs_Event& event, Auto_Up_Stairs_Context& context) override { + + if (event == Auto_Up_Stairs_Event::Down) { + return fsm.getState(Auto_Up_Stairs_State::Leg_Initial); + } + return nullptr; + } + Auto_Up_Stairs_State getStateID() const override { return Auto_Up_Stairs_State::Leg_Lift; } + +private: + rclcpp::Logger logger = rclcpp::get_logger("Lift_State"); +}; +class Auto_Up_Stairs { +public: + explicit Auto_Up_Stairs() { + lift.set_total_step(1600).set_end_point({0.0, -0.4, 0.2, 0.0, 0.0, 0.0}); + initial.set_total_step(2000).set_end_point({0.0, -0.9, 0.5, 0.0, 0.0, 0.0}); + fsm.registerState(); + fsm.registerState(); + fsm.start(Auto_Up_Stairs_State::Leg_Initial); + + fsm.addTransition( + Auto_Up_Stairs_State::Leg_Initial, Auto_Up_Stairs_Event::Up, + [this](const Auto_Up_Stairs_Event& event, const Auto_Up_Stairs_Context& context) { + if (event == Auto_Up_Stairs_Event::Up) { + if (initial.get_complete()) { + return true; + } + initial.set_start_point(last_theta_); + result = initial.trajectory(); + } + return false; + }, + [this](const Auto_Up_Stairs_Event& event, Auto_Up_Stairs_Context& context) {}, + Auto_Up_Stairs_State::Leg_Lift); + + fsm.addTransition( + Auto_Up_Stairs_State::Leg_Lift, Auto_Up_Stairs_Event::Up, + [this](const Auto_Up_Stairs_Event& event, const Auto_Up_Stairs_Context& context) { + if (event == Auto_Up_Stairs_Event::Up) { + lift.set_start_point(last_theta_); + result = lift.trajectory(); + } + + return false; + }, + [this](const Auto_Up_Stairs_Event& event, Auto_Up_Stairs_Context& context) {}, + Auto_Up_Stairs_State::Leg_Initial); + } + + void reset() { + fsm.start(Auto_Up_Stairs_State::Leg_Initial); + fsm_direction = initial_enter; + lift.reset(); + initial.reset(); + } + void get_current_theta(std::array theta) { last_theta_ = theta; } + Auto_Up_Stairs_State getState() { return fsm.getCurrentState(); } + std::array get_result() { return result; } + bool get_first_trajectory_result() { return initial.get_complete(); } + bool get_second_trajectory_result() { return lift.get_complete(); } + int fsm_direction = initial_enter; + void processEvent(const Auto_Up_Stairs_Event& event) { fsm.processEvent(event); } + +private: + std::array last_theta_, result; + Auto_Up_Stairs_Context context_; + + FiniteStateMachine fsm{ + context_}; + + rmcs_core::hardware::device::Trajectory + initial; + rmcs_core::hardware::device::Trajectory + lift; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_walk.hpp b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_walk.hpp new file mode 100644 index 00000000..b95d2adc --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/fsm/FSM_walk.hpp @@ -0,0 +1,78 @@ +#pragma once + +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/fsm/FSM.hpp" +#include "rclcpp/node.hpp" +#include +#include +#include +#include +#include +#include +#include + +enum class Auto_Walk_State { + Set_Walk_Arm, +}; +enum class Auto_Walk_Event { Up, Down }; +struct Auto_Walk_Context {}; + +class Set_Walk_Arm_State : public IState { +public: + void enter( + FiniteStateMachine& fsm, + const Auto_Walk_Context& context) override {} + void exit( + FiniteStateMachine& fsm, + const Auto_Walk_Context& context) override {} + std::shared_ptr> handleEvent( + FiniteStateMachine& fsm, + const Auto_Walk_Event& event, Auto_Walk_Context& context) override { + + if (event == Auto_Walk_Event::Up) { + return fsm.getState(Auto_Walk_State::Set_Walk_Arm); + } + return nullptr; + } + Auto_Walk_State getStateID() const override { return Auto_Walk_State::Set_Walk_Arm; } +}; + +class Auto_Set_Walk_Arm { +public: + explicit Auto_Set_Walk_Arm() { + fsm.registerState(); + fsm.addTransition( + Auto_Walk_State::Set_Walk_Arm, Auto_Walk_Event::Up, + [this](const Auto_Walk_Event& event, const Auto_Walk_Context& context) { + if (event == Auto_Walk_Event::Up) { + set_walk_arm.set_start_point(enter_theta_); + result = set_walk_arm.trajectory(); + } + return false; + }, + [this](const Auto_Walk_Event& event, Auto_Walk_Context& context) {}, + Auto_Walk_State::Set_Walk_Arm); + fsm.start(Auto_Walk_State::Set_Walk_Arm); + + set_walk_arm.set_total_step(2000).set_end_point({0, 1.04719, -1.0472, 0, -0.9, 0}); + } + void get_current_theta(std::array theta) { enter_theta_ = theta; } + std::array get_result() { return result; } + auto getState() { return fsm.getCurrentState(); } + void processEvent(const Auto_Walk_Event& event) { fsm.processEvent(event); } + + int fsm_direction = initial_enter; + void reset() { + fsm.start(Auto_Walk_State::Set_Walk_Arm); + set_walk_arm.reset(); + fsm_direction = initial_enter; + } + +private: + Auto_Walk_Context context_; + FiniteStateMachine fsm{context_}; + rmcs_core::hardware::device::Trajectory + set_walk_arm; + std::array enter_theta_, result; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM.hpp b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM.hpp new file mode 100644 index 00000000..9a8b6815 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM.hpp @@ -0,0 +1,238 @@ +#ifndef HARDWARE_HSM_HPP_H_ +#define HARDWARE_HSM_HPP_H_ +#include +#include +#include +#include +#include +#include +#include +#include + +// --------------------------- Variant / Event --------------------------- +using EventArgs = std::vector; + +template +struct Event { + EventId id; + EventArgs args; + + template + const T* getArg(size_t idx = 0) const { + if (idx >= args.size()) + return nullptr; + return std::any_cast(&args[idx]); + } +}; + +// --------------------------- IState (基础接口) --------------------------- +template +class IState { +public: + virtual ~IState() = default; + + // Called when HSM 启动并进入该状态 + virtual void enter(Context& ctx, const EventArgs& args) {} + + // Called当 HSM 离开该状态 + virtual void exit(Context& ctx) {} + + // 事件处理：返回 std::nullopt 表示“在本层无需切换父层状态”； + // 返回父层 StateId 表示父层状态需要切换为返回值 + virtual std::optional handleEvent(const Event& event, Context& ctx) = 0; + + // 当 HSM 注册这个 state 时被调用（给 state 一个机会去获得 Context 或构建子 HSM） + // 默认实现空，复合态可 override 在这里创建子 HSM 并注册子状态 + virtual void onRegister(Context& ctx) {} + + virtual StateId getStateID() const = 0; +}; + +// 前向声明 HSM，用于示例中 CompositeState 持有子 HSM 的类型名 +template +class HSM; + +// --------------------------- HSM（只负责当前层） --------------------------- +template +class HSM { +public: + explicit HSM(Context& ctx) + : context(ctx) {} + + // 注册状态（需要 StateType 有默认构造） + template + void registerState() { + static_assert( + std::is_base_of_v, StateType>, + "StateType must derive from IState"); + auto state = std::make_unique(); + // 在把 state 放入 map 之前，让 state 有机会初始化内部的子 HSM（如果需要） + state->onRegister(context); + + StateId id = state->getStateID(); + std::lock_guard lock(mtx); + states.emplace(id, std::move(state)); + } + + // 启动 HSM（进入初始态） + void start(StateId initial, const EventArgs& args = {}) { + std::lock_guard lock(mtx); + auto it = states.find(initial); + if (it == states.end()) { + std::cerr << "[HSM] Error: initial state not found\n"; + return; + } + currentState = it->second.get(); + if (currentState) + currentState->enter(context, args); + } + + // 停止 HSM（退出当前态并清空） + void stop() { + std::lock_guard lock(mtx); + if (currentState) { + currentState->exit(context); + currentState = nullptr; + } + } + + // 处理事件：返回 true 表示事件被处理（在本层或下层） + bool processEvent(const Event& event) { + std::lock_guard lock(mtx); + if (!currentState) + return false; + + // currentState 自己决定是否把事件交给子 HSM（如果它是 CompositeState） + auto nextId = currentState->handleEvent(event, context); + + // 如果子处理完或者当前态处理完但没有父层切换，则认为 handled = true + if (!nextId) { + return true; +// return false; + } + + // 若返回了一个父层 StateId（表示要切换本层状态） + if (*nextId == currentState->getStateID()) { + // 明确写出相同 ID 表示没有切换 + return true; + } + + // 切换状态 + auto it = states.find(*nextId); + if (it == states.end()) { + std::cerr << "[HSM] Target state not found\n"; + return true; // 认为已处理（避免事件漏出） + } + + currentState->exit(context); + currentState = it->second.get(); + currentState->enter(context, event.args); + return true; + } + + StateId getCurrentStateID() const { + std::lock_guard lock(mtx); + return currentState ? currentState->getStateID() : StateId{}; + } + +private: + Context& context; + mutable std::recursive_mutex mtx; + + std::unordered_map>> states; + IState* currentState = nullptr; +}; + +// --------------------------- CompositeState（模板：父 StateId + 子 StateId） +// --------------------------- +template +class CompositeState : public IState { +public: + CompositeState() + : innerHsm(nullptr) + , initialSubStatePtr(nullptr) + , is_subActive(false) {} + + // Derived 应在 override onRegister(ctx) 中：创建 innerHsm 和注册子状态 + // 示例： + // innerHsm = std::make_unique>(ctx); + // innerHsm->registerState(); + // innerHsm->registerState(); + // initialSubState = SubStateA; + // + void onRegister(Context& ctx) override { + // 默认不创建 innerHsm，派生类应创建 innerHsm（如果需要） + } + + // 进入复合态：激活子 HSM（如果已创建） + void enter(Context& ctx, const EventArgs& args) override { + if (innerHsm) { + is_subActive = true; + if (initialSubStatePtr) { + innerHsm->start(*initialSubStatePtr, args); + } + } + onEnter(ctx, args); + } + + // 退出复合态：停止子 HSM（如果存在） + void exit(Context& ctx) override { + if (innerHsm && is_subActive) { + innerHsm->stop(); + is_subActive = false; + } + onExit(ctx); + } + + // 事件处理：优先交给子 HSM；若子 HSM 未处理，再由 derived 处理（handleComposite） + std::optional handleEvent(const Event& event, Context& ctx) override { + if (innerHsm && is_subActive) { + bool handledByChild = innerHsm->processEvent(event); + if (handledByChild) { + // 子层处理完（或者在子层引发了子层内部转移），父层无需变更 + return std::nullopt; + } + else{ + return handleComposite(event, ctx); + } + } + // 子层未处理，则交给复合态自己的逻辑（由派生类实现） + throw std::runtime_error{"Unable to get parameter update_rate"}; + return std::nullopt; + } + + // 派生类可以在这里实现复合态在子层未处理时的行为（包括返回父层要切换的 StateId） + virtual std::optional handleComposite(const Event& /*event*/, Context& /*ctx*/) { + return std::nullopt; + } + +protected: + // helper：派生类在 onRegister 中可调用这些来创建与初始化子 HSM + void createInnerHsm(Context& ctx) { + innerHsm = std::make_unique>(ctx); + } + + template + void registerInnerState() { + if (!innerHsm) + throw std::runtime_error( + "Inner HSM not created. Call createInnerHsm(ctx) in onRegister first."); + innerHsm->template registerState(); + } + + void setInitialSubState(SubStateId id) { + initialSubState = id; + initialSubStatePtr = &initialSubState; + } + + // optional hooks for derived classes + virtual void onEnter(Context& /*ctx*/, const EventArgs& /*args*/) {} + virtual void onExit(Context& /*ctx*/) {} + +private: + std::unique_ptr> innerHsm; + SubStateId initialSubState; + SubStateId* initialSubStatePtr; // pointer used only when initialSubState is set + bool is_subActive; +}; +#endif // HARDWARE_HSM_HPP_H_ diff --git a/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_dev.hpp b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_dev.hpp new file mode 100644 index 00000000..73d5f9d5 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_dev.hpp @@ -0,0 +1,225 @@ +#ifndef HARDWARE_HSM_HPP_H_ +#define HARDWARE_HSM_HPP_H_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// --------------------------- Variant / Event --------------------------- +using EventArgs = std::vector; + +template +struct Event { + EventId id; + EventArgs args; + + template + const T* getArg(size_t idx = 0) const { + if (idx >= args.size()) + return nullptr; + return std::any_cast(&args[idx]); + } +}; + +// --------------------------- IState (基础接口) --------------------------- +template +class IState { +public: + virtual ~IState() = default; + + virtual void enter(Context& ctx, const EventArgs& args) {} + + virtual void exit(Context& ctx) {} + + virtual std::optional handleEvent(const Event& event, Context& ctx) = 0; + + virtual void onRegister(Context& ctx) {} + + virtual StateId getStateID() const = 0; +}; + +// 前向声明 HSM +template +class HSM; + +// --------------------------- HSM（只负责当前层） --------------------------- +template +class HSM { +public: + explicit HSM(Context& ctx) + : context(ctx) {} + + void registerState(std::unique_ptr> state) { + state->onRegister(context); + StateId id = state->getStateID(); + std::lock_guard lock(mtx); + states.emplace(id, std::move(state)); + } + + void start(StateId initial, const EventArgs& args = {}) { + std::lock_guard lock(mtx); + auto it = states.find(initial); + if (it == states.end()) { + std::cerr << "[HSM] Error: initial state not found\n"; + return; + } + currentState = it->second.get(); + if (currentState) + currentState->enter(context, args); + } + + void stop() { + std::lock_guard lock(mtx); + if (currentState) { + currentState->exit(context); + currentState = nullptr; + } + } + + bool processEvent(const Event& event) { + std::lock_guard lock(mtx); + if (!currentState) + return false; + + auto nextId = currentState->handleEvent(event, context); + + if (!nextId) { + return true; + } + + if (*nextId == currentState->getStateID()) { + return true; + } + + auto it = states.find(*nextId); + if (it == states.end()) { + std::cerr << "[HSM] Target state not found\n"; + return true; + } + + currentState->exit(context); + currentState = it->second.get(); + currentState->enter(context, event.args); + return true; + } + + StateId getCurrentStateID() const { + std::lock_guard lock(mtx); + return currentState ? currentState->getStateID() : StateId{}; + } + +private: + Context& context; + mutable std::recursive_mutex mtx; + + std::unordered_map>> states; + IState* currentState = nullptr; +}; + +// --------------------------- BasicState for simplified state creation --------------------------- +template +using EnterFunc = std::function; +template +using ExitFunc = std::function; +template +using HandleFunc = std::function(const Event&, Context&)>; + +template +class BasicState : public IState { +public: + BasicState(StateId id, EnterFunc enter, ExitFunc exit, HandleFunc handle) + : id_(id), enter_(std::move(enter)), exit_(std::move(exit)), handle_(std::move(handle)) {} + + StateId getStateID() const override { return id_; } + + void enter(Context& ctx, const EventArgs& args) override { + if (enter_) enter_(ctx, args); + } + + void exit(Context& ctx) override { + if (exit_) exit_(ctx); + } + + std::optional handleEvent(const Event& e, Context& ctx) override { + return handle_ ? handle_(e, ctx) : std::nullopt; + } + +private: + StateId id_; + EnterFunc enter_; + ExitFunc exit_; + HandleFunc handle_; +}; + +// --------------------------- CompositeState（支持动态子状态注册） --------------------------- +template +class CompositeState : public IState { +public: + CompositeState(ParentStateId id, SubStateId initialSub) + : id_(id), initialSubState(initialSub), innerHsm(nullptr), is_subActive(false) {} + + void addSubState(std::unique_ptr> subState) { + subStatesToRegister.push_back(std::move(subState)); + } + + void onRegister(Context& ctx) override { + innerHsm = std::make_unique>(ctx); + for (auto& sub : subStatesToRegister) { + innerHsm->registerState(std::move(sub)); + } + subStatesToRegister.clear(); // 清空以释放内存 + onEnterRegister(ctx); + } + + void enter(Context& ctx, const EventArgs& args) override { + if (innerHsm) { + is_subActive = true; + innerHsm->start(initialSubState, args); + } + onEnter(ctx, args); + } + + void exit(Context& ctx) override { + if (innerHsm && is_subActive) { + innerHsm->stop(); + is_subActive = false; + } + onExit(ctx); + } + + std::optional handleEvent(const Event& event, Context& ctx) override { + if (innerHsm && is_subActive) { + bool handledByChild = innerHsm->processEvent(event); + if (handledByChild) { + return std::nullopt; + } + } + return handleComposite(event, ctx); + } + + ParentStateId getStateID() const override { return id_; } + +protected: + virtual void onEnterRegister(Context& /*ctx*/) {} + virtual void onEnter(Context& /*ctx*/, const EventArgs& /*args*/) {} + virtual void onExit(Context& /*ctx*/) {} + virtual std::optional handleComposite(const Event& /*event*/, Context& /*ctx*/) { + return std::nullopt; + } + +private: + ParentStateId id_; + SubStateId initialSubState; + std::unique_ptr> innerHsm; + std::vector>> subStatesToRegister; + bool is_subActive; +}; + +#endif // HARDWARE_HSM_HPP_H_ \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_up_stairs_dev.hpp b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_up_stairs_dev.hpp new file mode 100644 index 00000000..e658e978 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/hsm/HSM_up_stairs_dev.hpp @@ -0,0 +1,603 @@ +#ifndef HSM_UP_TWO_STAIRS_HPP_H_ +#define HSM_UP_TWO_STAIRS_HPP_H_ + +#include "hardware/device/trajectory.hpp" +#include "hardware/hsm/HSM_dev.hpp" // 注意: 使用优化后的 HSM.hpp +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using InputD = rmcs_executor::Component::InputInterface*; +using InputVct = + rmcs_executor::Component::InputInterface*; + +enum class UpStairsState { Initial, StepByOne, StepByTwo_initial, StepByTwo_lift }; + +enum class StepSubState { + Wait, + Press, + Lift, + LiftAndInitial, + InitialAgain, + Delay, + PressAgain, + LiftAgain +}; + +enum class UpStairsEventEnum { + quit, + stop, + tick, + go_to_OneProcess, + go_to_TwoProcess_lift, + go_to_TwoProcess_initial, + go_to_Lift_And_Initial, + go_to_Press, + go_to_Lift, + tof_already, // args:: double distance + go_to_Initial_Again +}; + +using UpStairsEventId = std::variant; + +namespace rmcs_core::hardware::hsm::up_stairs_hsm { + +namespace events { +using UpStairsEvent = Event; +const UpStairsEvent tick{UpStairsEventId{UpStairsEventEnum::tick}, {}}; + +const UpStairsEvent quit{UpStairsEventId{UpStairsEventEnum::quit}, {}}; + +const UpStairsEvent stop{UpStairsEventId{UpStairsEventEnum::stop}, {}}; + +const UpStairsEvent go_to_OneProcess{UpStairsEventId{UpStairsEventEnum::go_to_OneProcess}, {}}; + +const UpStairsEvent go_to_TwoProcess_lift{ + UpStairsEventId{UpStairsEventEnum::go_to_TwoProcess_lift}, {}}; + +const UpStairsEvent go_to_TwoProcess_initial{ + UpStairsEventId{UpStairsEventEnum::go_to_TwoProcess_initial}, {}}; + +const UpStairsEvent go_to_Lift_And_Initial{ + UpStairsEventId{UpStairsEventEnum::go_to_Lift_And_Initial}, {}}; + +const UpStairsEvent go_to_Press{UpStairsEventId{UpStairsEventEnum::go_to_Press}, {}}; + +const UpStairsEvent go_to_Lift{UpStairsEventId{UpStairsEventEnum::go_to_Lift}, {}}; + +const UpStairsEvent go_to_Initial_Again{ + UpStairsEventId{UpStairsEventEnum::go_to_Initial_Again}, {}}; + +} // namespace events + +struct UpStairsContext { + std::unordered_map data; // 动态存储所有依赖 + + // Helper getters + template + T& get(const std::string& key) { + auto it = data.find(key); + if (it == data.end()) { + throw std::runtime_error("Key not found: " + key); + } + if (it->second.type() != typeid(std::reference_wrapper)) { + throw std::runtime_error( + "Type mismatch for key: " + key + ", stored=" + it->second.type().name() + + ", requested=" + typeid(T).name()); + } + return std::any_cast>(it->second).get(); + } + + // 示例: 获取 Trajectory + hardware::device::Trajectory& + getTrajectory(const std::string& key) { + return get>(key); + } + + // 获取 InputD* + InputD& getInputD(const std::string& key) { return get(key); } + + // 获取 InputVct* + InputVct& getInputVct(const std::string& key) { return get(key); } + + // 其他: k, b, result 等 + std::vector& getVec(const std::string& key) { return get>(key); } + + std::array& getResult() { return get>("result"); } + + static constexpr double v_reference = 1.5; + bool is_one_process = false; + bool is_two_process_lift = false; + bool is_two_process_initial = false; + int calculate_steps(double k, double b) { + double raw = k * ((**getInputVct("speed"))->x() - v_reference) + b; + return (std::max(static_cast(raw), 200)); + } +}; + +class Auto_Leg_Up_Two_Stairs : public rclcpp::Node { +public: + explicit Auto_Leg_Up_Two_Stairs() + : context_() + , up_stairs_hsm(context_) + , Node{"auto_leg_up_stairs"} {} + + void processEvent(const Event& event) { + // 处理事件ID为variant: 使用std::visit + std::visit( + [&](auto&& id) { + using T = std::decay_t; + + if constexpr (std::is_same_v) { + //RCLCPP_INFO(this->get_logger(), "Processing event: %d", static_cast(id)); + } else if constexpr (std::is_same_v) { + //RCLCPP_INFO(this->get_logger(), "Processing custom event: %s", id.c_str()); + } + }, + event.id); + up_stairs_hsm.processEvent(event); + } + + void start(UpStairsState initial, const EventArgs& args = {}) { + check_context_ready(); + up_stairs_hsm.start(initial, args); + } + + void stop() { up_stairs_hsm.stop(); } + + void check_context_ready() const { + // 动态检查关键key是否存在 + const std::vector required_keys = {"theta_lf", "theta_lb", "theta_rb", + "theta_rf", "speed", "result"}; + for (const auto& key : required_keys) { + if (context_.data.find(key) == context_.data.end()) { + throw std::runtime_error(key + " not bound"); + } + } + } + + template + Auto_Leg_Up_Two_Stairs& bind(const std::string& key, T& value) { + context_.data[key] = std::ref(value); + return *this; + } + + std::array& get_result() { return context_.getResult(); } + + Auto_Leg_Up_Two_Stairs& init_and_trajectory_set( + const std::vector& initial_end_point_, const std::vector& press_end_point_, + const std::vector& lift_end_point_, + const std::vector& lift_and_initial_end_point_, + const std::vector& initial_again_end_point_) { + + // 绑定轨迹 (作为成员变量，但现在动态bind) + bind("initial", up_stairs_initial) + .bind("press", up_stairs_leg_press) + .bind("lift", up_stairs_leg_lift) + .bind("lift_and_initial", up_stairs_leg_lift_and_initial) + .bind("initial_again", up_stairs_leg_initial_again); + + // 设置端点 + up_stairs_initial.set_end_point( + {initial_end_point_[0], initial_end_point_[1], initial_end_point_[2], + initial_end_point_[3], 0, 0}); + up_stairs_leg_press.set_end_point( + {press_end_point_[0], press_end_point_[1], press_end_point_[2], press_end_point_[3], 0, + 0}); + up_stairs_leg_lift.set_end_point( + {lift_end_point_[0], lift_end_point_[1], lift_end_point_[2], lift_end_point_[3], 0, 0}); + up_stairs_leg_lift_and_initial.set_end_point( + {lift_and_initial_end_point_[0], lift_and_initial_end_point_[1], + lift_and_initial_end_point_[2], lift_and_initial_end_point_[3], 0, 0}); + up_stairs_leg_initial_again.set_end_point( + {initial_again_end_point_[0], initial_again_end_point_[1], initial_again_end_point_[2], + initial_again_end_point_[3], 0, 0}); + + // 注册状态使用BasicState和lambda + + // InitialState + auto initialState = + std::make_unique>( + UpStairsState::Initial, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in initial"); + auto& traj = ctx.getTrajectory("initial"); + + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(1000); + }, + nullptr, // no exit + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("initial"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return UpStairsState::Initial; + } else { + return std::nullopt; + } + } else if (id == UpStairsEventEnum::go_to_TwoProcess_lift) { + ctx.is_one_process = false; + ctx.is_two_process_lift = true; + ctx.is_two_process_initial = false; + return UpStairsState::StepByTwo_lift; + } else if (id == UpStairsEventEnum::go_to_TwoProcess_initial) { + ctx.is_one_process = false; + ctx.is_two_process_lift = false; + ctx.is_two_process_initial = true; + return UpStairsState::StepByTwo_initial; + } else if (id == UpStairsEventEnum::go_to_OneProcess) { + ctx.is_one_process = true; + ctx.is_two_process_lift = false; + ctx.is_two_process_initial = false; + return UpStairsState::StepByOne; + } + } + return std::nullopt; + }, + event.id); + }); + up_stairs_hsm.registerState(std::move(initialState)); + + // StepByOneState (Composite) + auto stepByOne = std::make_unique< + CompositeState>( + UpStairsState::StepByOne, StepSubState::Press); + + // 添加子状态动态 + + stepByOne->addSubState(createPressState()); + stepByOne->addSubState(createLiftState()); + stepByOne->addSubState(createWaitState()); + up_stairs_hsm.registerState(std::move(stepByOne)); + + // StepByTwoState (Composite) + auto stepByTwo_initial = std::make_unique< + CompositeState>( + UpStairsState::StepByTwo_initial, StepSubState::Press); + + stepByTwo_initial->addSubState(createPressState()); + stepByTwo_initial->addSubState(createLiftState()); + stepByTwo_initial->addSubState(createInitialAgainState()); + stepByTwo_initial->addSubState(createPressAgainState()); + stepByTwo_initial->addSubState(createLiftAgainState()); + stepByTwo_initial->addSubState(createWaitState()); + up_stairs_hsm.registerState(std::move(stepByTwo_initial)); + + auto stepByTwo_lift = std::make_unique< + CompositeState>( + UpStairsState::StepByTwo_lift, StepSubState::Press); + + stepByTwo_lift->addSubState(createPressState()); + stepByTwo_lift->addSubState(createLiftAndInitialState()); + stepByTwo_lift->addSubState(createPressAgainState()); + stepByTwo_lift->addSubState(createLiftAgainState()); + stepByTwo_lift->addSubState(createWaitState()); + up_stairs_hsm.registerState(std::move(stepByTwo_lift)); + return *this; + } + +private: + // Helper functions to create sub states + std::unique_ptr> createWaitState() { + return std::make_unique>( + StepSubState::Wait, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in wait"); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::go_to_Lift_And_Initial) { + return StepSubState::LiftAndInitial; + } else if (id == UpStairsEventEnum::go_to_Press) { + return StepSubState::Press; + } else if (id == UpStairsEventEnum::go_to_Lift) { + return StepSubState::Lift; + } else if (id == UpStairsEventEnum::tick) { + return StepSubState::Wait; + } else if (id == UpStairsEventEnum::go_to_Initial_Again) { + return StepSubState::InitialAgain; + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> createDelayState() { + return std::make_unique>( + StepSubState::Delay, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), "in delay"); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::go_to_Lift_And_Initial) { + return StepSubState::LiftAndInitial; + } else if (id == UpStairsEventEnum::go_to_Press) { + return StepSubState::Press; + } else if (id == UpStairsEventEnum::go_to_Lift) { + return StepSubState::Lift; + } else if (id == UpStairsEventEnum::tick) { + return StepSubState::Wait; + } else if (id == UpStairsEventEnum::go_to_Initial_Again) { + return StepSubState::InitialAgain; + } else if (id == UpStairsEventEnum::go_to_Lift_And_Initial) { + return StepSubState::LiftAndInitial; + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> createPressState() { + return std::make_unique>( + StepSubState::Press, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), "in press"); + auto& traj = ctx.getTrajectory("press"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[0], ctx.getVec("b")[0])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("press"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::Press; + } else { + if (ctx.is_one_process) { + return StepSubState::Lift; + } else if (ctx.is_two_process_initial) { + return StepSubState::Lift; + } else if (ctx.is_two_process_lift) { + return StepSubState::LiftAndInitial; + } + } + } + } + return std::nullopt; + }, + event.id); + }); + } + std::unique_ptr> + createLiftAndInitialState() { + return std::make_unique>( + StepSubState::LiftAndInitial, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in lift_and_initial"); + auto& traj = ctx.getTrajectory("lift_and_initial"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[3], ctx.getVec("b")[3])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("lift_and_initial"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::LiftAndInitial; + } else { + // return StepSubState::Wait; + return StepSubState::PressAgain; + } + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> createLiftState() { + return std::make_unique>( + StepSubState::Lift, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in lift"); + auto& traj = ctx.getTrajectory("lift"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[1], ctx.getVec("b")[1])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("lift"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::Lift; + } else { + if (ctx.is_one_process) { + return StepSubState::Wait; + } else if (ctx.is_two_process_initial) { + return StepSubState::InitialAgain; + } else if (ctx.is_two_process_lift) { + RCLCPP_INFO( + this->get_logger(), + "wrongly enter lift in two_process_lift"); + } + } + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> + createInitialAgainState() { + return std::make_unique>( + StepSubState::InitialAgain, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), "in initial_again"); + auto& traj = ctx.getTrajectory("initial_again"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[2], ctx.getVec("b")[2])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("initial_again"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::InitialAgain; + } else { + return StepSubState::PressAgain; + } + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> + createPressAgainState() { + return std::make_unique>( + StepSubState::PressAgain, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in press again"); + auto& traj = ctx.getTrajectory("press"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[0], ctx.getVec("b")[0])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("press"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::PressAgain; + } else { + return StepSubState::LiftAgain; + } + } + } + return std::nullopt; + }, + event.id); + }); + } + + std::unique_ptr> createLiftAgainState() { + return std::make_unique>( + StepSubState::LiftAgain, + [&](UpStairsContext& ctx, const EventArgs&) { + RCLCPP_INFO(this->get_logger(), " in lift again"); + auto& traj = ctx.getTrajectory("lift"); + traj.reset(); + traj.set_start_point( + {**ctx.getInputD("theta_lf"), **ctx.getInputD("theta_lb"), + **ctx.getInputD("theta_rb"), **ctx.getInputD("theta_rf"), 0.0, 0.0}); + traj.set_total_step(ctx.calculate_steps(ctx.getVec("k")[1], ctx.getVec("b")[1])); + }, + nullptr, + [&](const Event& event, + UpStairsContext& ctx) -> std::optional { + return std::visit( + [&](auto&& id) -> std::optional { + using T = std::decay_t; + if constexpr (std::is_same_v) { + if (id == UpStairsEventEnum::tick) { + auto& traj = ctx.getTrajectory("lift"); + if (!traj.get_complete()) { + ctx.getResult() = traj.trajectory(); + return StepSubState::LiftAgain; + } else { + return StepSubState::Wait; + } + } + } + return std::nullopt; + }, + event.id); + }); + } + + hardware::device::Trajectory up_stairs_initial; + hardware::device::Trajectory up_stairs_leg_press; + hardware::device::Trajectory up_stairs_leg_lift; + hardware::device::Trajectory + up_stairs_leg_lift_and_initial; + hardware::device::Trajectory + up_stairs_leg_initial_again; + + UpStairsContext context_; + HSM up_stairs_hsm; +}; + +} // namespace rmcs_core::hardware::hsm::up_stairs_hsm + +#endif // HSM_UP_TWO_STAIRS_HPP_H_ \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/test.cpp b/rmcs_ws/src/rmcs_core/src/hardware/test.cpp new file mode 100644 index 00000000..17789857 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/test.cpp @@ -0,0 +1,112 @@ +#include "hardware/device/tof.hpp" +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::hardware { + +class Test + : public rmcs_executor::Component + , public rclcpp::Node { + +public: + Test() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , command_component_( + create_partner_component(get_component_name() + "_command", *this)) { + using namespace rmcs_description; + bottom_board_ = std::make_unique( + *this, *command_component_, get_logger(), + static_cast(get_parameter("usb_pid_bottom_board").as_int())); + } + + ~Test() override = default; + + void command_update() { bottom_board_->command_update(); } + + void before_updating() override { bottom_board_->before_updating(); } + void update() override { bottom_board_->update(); } + +private: + class TestCommand : public rmcs_executor::Component { + public: + explicit TestCommand(Test& test) + : test_(test) {} + + void update() override { test_.command_update(); } + + Test& test_; + }; + + std::shared_ptr command_component_; + + class BottomBoard final : private librmcs::client::CBoard { + public: + friend class Test; + explicit BottomBoard( + Test& test, TestCommand& test_command, rclcpp::Logger logger, int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , logger_(std::move(logger)) + , tof_(test, "/tof") + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) {} + + ~BottomBoard() final { + stop_handling_events(); + event_thread_.join(); + } + void before_updating() {} + void update() { + // RCLCPP_INFO(logger_, "Received UART1 data"); + + tof_.update(); + } + void command_update() {} + + private: + void uart2_receive_callback(const std::byte* data, uint8_t length) override { + + RCLCPP_INFO(logger_, "Received UART2 length:%x",length); + if (length < 6) { + RCLCPP_INFO(logger_, "too short length: %x", length); + return; + } + + + if (std::to_integer(data[length - 1]) != 0x0A) { + RCLCPP_INFO( + logger_, "Invalid back: 0x%02x", std::to_integer(data[length - 1])); + + return; + } + + RCLCPP_INFO( + logger_, " 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x", + std::to_integer(data[0]), std::to_integer(data[1]), + std::to_integer(data[2]), std::to_integer(data[3]), + std::to_integer(data[4]), std::to_integer(data[5]), + std::to_integer(data[6])); + + tof_.store_status(data, length); + } + + rclcpp::Logger logger_; + + rmcs_core::hardware::device::Tof tof_; + librmcs::utility::RingBuffer ring_buff{16}; + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + }; + + std::unique_ptr bottom_board_; +}; + +} // namespace rmcs_core::hardware +#include +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Test, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/lk_robot/arm.cpp b/rmcs_ws/src/rmcs_core/src/lk_robot/arm.cpp new file mode 100644 index 00000000..fc05081d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/lk_robot/arm.cpp @@ -0,0 +1,148 @@ +#include "hardware/device/Kinematic.hpp" +#include "hardware/device/drag_teach.hpp" +#include "hardware/device/trajectory.hpp" +#include "hardware/endian_promise.hpp" +#include "hardware/fsm/FSM.hpp" +#include "hardware/fsm/FSM_gold_l.hpp" +#include "hardware/fsm/FSM_gold_m.hpp" +#include "hardware/fsm/FSM_gold_r.hpp" +#include "hardware/fsm/FSM_sliver.hpp" +#include "hardware/fsm/FSM_storage_lb.hpp" +#include "hardware/fsm/FSM_storage_rb.hpp" +#include "hardware/fsm/FSM_up_stairs.hpp" +#include "hardware/fsm/FSM_walk.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::arm { +class Arm + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Arm() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/arm/Joint6/theta", theta[5]); + register_input("/arm/Joint5/theta", theta[4]); + register_input("/arm/Joint4/theta", theta[3]); + register_input("/arm/Joint3/theta", theta[2]); + register_input("/arm/Joint2/theta", theta[1]); + register_input("/arm/Joint1/theta", theta[0]); + register_output("/arm/Joint6/target_theta", target_theta[5], nan); + register_output("/arm/Joint5/target_theta", target_theta[4], nan); + register_output("/arm/Joint4/target_theta", target_theta[3], nan); + register_output("/arm/Joint3/target_theta", target_theta[2], nan); + register_output("/arm/Joint2/target_theta", target_theta[1], nan); + register_output("/arm/Joint1/target_theta", target_theta[0], nan); + register_output("/arm/enable_flag", is_arm_enable, false); + }; + void update() override { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + using namespace rmcs_msgs; + if (!initial_check_done_) { + *is_arm_enable = false; + reset_motors(); + // RCLCPP_INFO(get_logger(), "awwwwww"); + if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + + initial_check_done_ = true; + } + } else { + // RCLCPP_INFO(get_logger(), "asasa"); + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + *is_arm_enable = false; + reset_motors(); + + } else { + *is_arm_enable = true; + } + + if (switch_left == Switch::UP && switch_right == Switch::MIDDLE) { + execute_dt7_orientation(); + } else if (switch_left == Switch::UP && switch_right == Switch::UP) { + execute_dt7_position(); + } + } + } + +private: + void execute_dt7_orientation() { + if (fabs(joystick_left_->y()) > 0.01) { + *target_theta[5] += 0.003 * joystick_left_->y(); + *target_theta[5] = std::clamp(*target_theta[5], -3.1415926, 3.1415926); + } + if (fabs(joystick_left_->x()) > 0.01) { + *target_theta[4] += 0.003 * joystick_left_->x(); + *target_theta[4] = std::clamp(*target_theta[4], -1.83532, 1.83532); + } + if (fabs(joystick_right_->y()) > 0.01) { + *target_theta[3] += 0.003 * joystick_right_->y(); + *target_theta[3] = std::clamp(*target_theta[3], -3.1415926, 3.1415926); + } + } + void execute_dt7_position() { + if (fabs(joystick_left_->x()) > 0.01) { + *target_theta[2] += 0.002 * joystick_left_->x(); + *target_theta[2] = std::clamp(*target_theta[2], -1.01, 0.9227); + } + if (fabs(joystick_right_->x()) > 0.01) { + *target_theta[1] += 0.002 * joystick_right_->x(); + *target_theta[1] = std::clamp(*target_theta[1], -1.0108, 1.09719); + } + if (fabs(joystick_left_->y()) > 0.01) { + *target_theta[0] += 0.002 * joystick_left_->y(); + *target_theta[0] = std::clamp(*target_theta[0], -2.841592, 2.841592); + } + } + void reset_motors() { + *is_arm_enable = false; + *target_theta[5] = *theta[5]; + *target_theta[4] = *theta[4]; + *target_theta[3] = *theta[3]; + *target_theta[2] = *theta[2]; + *target_theta[1] = *theta[1]; + *target_theta[0] = *theta[0]; + } + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + OutputInterface is_arm_enable; + InputInterface theta[6]; // motor_current_angle + OutputInterface target_theta[6]; + static constexpr double nan = std::numeric_limits::quiet_NaN(); + int delay = 0; + bool initial_check_done_ = false; +}; +} // namespace rmcs_core::controller::arm + // namespace rmcs_core::controller::arm +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::arm::Arm, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/lk_robot/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/lk_robot/chassis_controller.cpp new file mode 100644 index 00000000..0737a134 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/lk_robot/chassis_controller.cpp @@ -0,0 +1,193 @@ +#include "controller/pid/pid_calculator.hpp" +#include "hardware/device/trajectory.hpp" +#include "rmcs_msgs/arm_mode.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::controller::chassis { +class Steering + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Steering() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + + register_input("/steering/steering/lf/angle", steering_lf_angle); + register_input("/steering/steering/lb/angle", steering_lb_angle); + register_input("/steering/steering/rb/angle", steering_rb_angle); + register_input("/steering/steering/rf/angle", steering_rf_angle); + + register_output( + "/steering/steering/lf/target_angle_error", steering_lf_target_angle_error, NAN); + register_output( + "/steering/steering/lb/target_angle_error", steering_lb_target_angle_error, NAN); + register_output( + "/steering/steering/rb/target_angle_error", steering_rb_target_angle_error, NAN); + register_output( + "/steering/steering/rf/target_angle_error", steering_rf_target_angle_error, NAN); + + register_output("/steering/wheel/lf/target_vel", steering_wheel_lf_target_vel, NAN); + register_output("/steering/wheel/lb/target_vel", steering_wheel_lb_target_vel, NAN); + register_output("/steering/wheel/rb/target_vel", steering_wheel_rb_target_vel, NAN); + register_output("/steering/wheel/rf/target_vel", steering_wheel_rf_target_vel, NAN); + } + void update() override { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + using namespace rmcs_msgs; + + if (!initial_check_done_) { + reset_motor(); + + // RCLCPP_INFO(get_logger(), "awwwwww"); + if (switch_left == Switch::DOWN && switch_right == Switch::DOWN) { + + initial_check_done_ = true; + } + } else { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_motor(); + + } else { + if (switch_left == Switch::MIDDLE && switch_right == Switch::MIDDLE) { + Eigen::Vector2d move_ = -*joystick_left_; + steering_control(move_, -joystick_right_->y() / 2.3); + } + // Eigen::Rotation2D rotation( + *joint1_theta); + // Eigen::Vector2d move_ = rotation * (*joystick_left_); + } + } + } + +private: + void steering_control(const Eigen::Vector2d& move, double spin_speed) { + + Eigen::Vector2d lf_vel = Eigen::Vector2d{-spin_speed, spin_speed} + move; + Eigen::Vector2d lb_vel = Eigen::Vector2d{-spin_speed, -spin_speed} + move; + Eigen::Vector2d rb_vel = Eigen::Vector2d{spin_speed, -spin_speed} + move; + Eigen::Vector2d rf_vel = Eigen::Vector2d{spin_speed, spin_speed} + move; + + Eigen::Vector2d lb_vel_angle = lb_vel; + Eigen::Vector2d lf_vel_angle = lf_vel; + Eigen::Vector2d rb_vel_angle = rb_vel; + Eigen::Vector2d rf_vel_angle = rf_vel; + + double err[4] = { + -atan2(lb_vel_angle.y(), lb_vel_angle.x()) - *steering_lb_angle, + -atan2(lf_vel_angle.y(), lf_vel_angle.x()) - *steering_lf_angle, + -atan2(rb_vel_angle.y(), rb_vel_angle.x()) - *steering_rb_angle, + -atan2(rf_vel_angle.y(), rf_vel_angle.x()) - *steering_rf_angle}; + + *steering_lf_target_angle_error = norm_error_angle(err[1]); + *steering_lb_target_angle_error = norm_error_angle(err[0]); + *steering_rb_target_angle_error = norm_error_angle(err[2]); + *steering_rf_target_angle_error = norm_error_angle(err[3]); + *steering_wheel_lf_target_vel = + lf_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[1]); + *steering_wheel_lb_target_vel = + lb_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[0]); + *steering_wheel_rb_target_vel = + rb_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[2]); + *steering_wheel_rf_target_vel = + rf_vel.norm() * (speed_limit / wheel_r) * check_error_angle(err[3]); + } + void reset_motor() { + *steering_lf_target_angle_error = NAN; + *steering_lb_target_angle_error = NAN; + *steering_rb_target_angle_error = NAN; + *steering_rf_target_angle_error = NAN; + + *steering_wheel_lf_target_vel = NAN; + *steering_wheel_lb_target_vel = NAN; + *steering_wheel_rb_target_vel = NAN; + *steering_wheel_rf_target_vel = NAN; + } + static inline double norm_error_angle(const double& angle) { + double tmp = angle; + while (tmp > 2 * M_PI) + tmp -= 2 * M_PI; + while (tmp <= 0) + tmp += 2 * M_PI; + if (tmp > 2 * M_PI - tmp) + tmp = tmp - 2 * M_PI; + if (tmp > M_PI / 2) + tmp = tmp - M_PI; + else if (tmp < -M_PI / 2) + tmp = tmp + M_PI; + return tmp; + } + static inline double check_error_angle(const double& angle) { + double tmp = angle; + while (tmp > 2 * M_PI) + tmp -= 2 * M_PI; + while (tmp <= 0) + tmp += 2 * M_PI; + tmp -= M_PI; + + return sin(abs(tmp) - M_PI_2); + } + static double normalizeAngle(double angle) { + + while (angle > M_PI) + angle -= 2 * M_PI; + while (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + double speed_limit = 2.5; // m/s + static constexpr double wheel_r = 0.11; // m + + const Eigen::Vector2d lf_vel_{1, -1}; + const Eigen::Vector2d lb_vel_{1, 1}; + const Eigen::Vector2d rb_vel_{-1, -1}; + const Eigen::Vector2d rf_vel_{-1, 1}; + Eigen::Vector2d lf_vel_last_{1, -1}; + Eigen::Vector2d lb_vel_last_{1, 1}; + Eigen::Vector2d rb_vel_last_{-1, -1}; + Eigen::Vector2d rf_vel_last_{-1, 1}; + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface steering_lf_angle; + InputInterface steering_lb_angle; + InputInterface steering_rb_angle; + InputInterface steering_rf_angle; + + OutputInterface steering_lf_target_angle_error; + OutputInterface steering_lb_target_angle_error; + OutputInterface steering_rb_target_angle_error; + OutputInterface steering_rf_target_angle_error; + OutputInterface steering_wheel_lf_target_vel; + OutputInterface steering_wheel_lb_target_vel; + OutputInterface steering_wheel_rb_target_vel; + OutputInterface steering_wheel_rf_target_vel; + bool initial_check_done_ = false; +}; +} // namespace rmcs_core::controller::chassis +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::Steering, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/lk_robot/lk_robot.cpp b/rmcs_ws/src/rmcs_core/src/lk_robot/lk_robot.cpp new file mode 100644 index 00000000..80e0e773 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/lk_robot/lk_robot.cpp @@ -0,0 +1,504 @@ +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/encorder.hpp" +#include "hardware/device/joint.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/tof.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace rmcs_core::hardware { + +class LK_Robot + : public rmcs_executor::Component + , public rclcpp::Node { + +public: + LK_Robot() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , command_component_( + create_partner_component(get_component_name() + "_command", *this)) + , right_board_( + *this, *command_component_, this->get_logger(), + static_cast(get_parameter("right_board_usb_pid").as_int())) + , left_board_( + *this, *command_component_, this->get_logger(), + static_cast(get_parameter("left_board_usb_pid").as_int())) {} + + ~LK_Robot() override = default; + + void command() { + right_board_.command(); + left_board_.command(); + } + void update() override { + right_board_.update(); + left_board_.update(); + } + +private: + class LK_Robot_command : public rmcs_executor::Component { + public: + explicit LK_Robot_command(LK_Robot& LK_Robot) + : LK_Robot_(LK_Robot) {} + + void update() override { LK_Robot_.command(); } + + LK_Robot& LK_Robot_; + }; + + std::shared_ptr command_component_; + + class right_board final : private librmcs::client::CBoard { + public: + friend class LK_Robot; + explicit right_board( + LK_Robot& LK_Robot, LK_Robot_command& LK_Robot_command, rclcpp::Logger logger, + int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , logger_(std::move(logger)) + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) + , dr16_(LK_Robot) + , Steering_Motors( + {LK_Robot, LK_Robot_command, "/steering/steering/rf"}, + {LK_Robot, LK_Robot_command, "/steering/steering/rb"}) + , Wheel_Motors( + {LK_Robot, LK_Robot_command, "/steering/wheel/rf"}, + {LK_Robot, LK_Robot_command, "/steering/wheel/rb"}) + , joint( + {LK_Robot, LK_Robot_command, "/arm/Joint4"}, + {LK_Robot, LK_Robot_command, "/arm/Joint5"}, + {LK_Robot, LK_Robot_command, "/arm/Joint6"}) { + + LK_Robot.register_output("/arm/Joint6/control_angle_error", joint6_error_angle); + LK_Robot.register_output("/arm/Joint5/control_angle_error", joint5_error_angle); + LK_Robot.register_output("/arm/Joint4/control_angle_error", joint4_error_angle); + + LK_Robot_command.register_input("/arm/enable_flag", is_arm_enable_); + + Wheel_Motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508} + .set_reduction_ratio(11.0).reverse() + ); + Wheel_Motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508} + .set_reduction_ratio(11.0).reverse() + ); + Steering_Motors[0].configure( + device::LKMotorConfig{device::LKMotorType::MHF7015} + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast( + LK_Robot.get_parameter("right_forward_zero_point").as_int())) + .enable_multi_turn_angle()); + Steering_Motors[1].configure( + device::LKMotorConfig{device::LKMotorType::MHF7015} + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("right_back_zero_point").as_int())) + .enable_multi_turn_angle()); + using namespace device; + joint[2].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i10V3}.set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("joint6_zero_point").as_int())), + DHConfig{0, -0.0571, 0, 0}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint6_qlim").as_double_array()}); + joint[1].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i10V3} + .enable_multi_turn_angle() + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast( + LK_Robot.get_parameter("joint5_zero_point").as_int())), + DHConfig{0, 0, 1.5707963, 0}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint5_qlim").as_double_array()}); + joint[0].configure_joint( + LKMotorConfig{LKMotorType::MG4010E_i36V3}.set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("joint4_zero_point").as_int())), + DHConfig{0, 0.33969, 1.5707963, 0}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint4_qlim").as_double_array()}); + uint64_t command; + command = device::LKMotor::lk_quest_command(); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(command)); + + transmit_buffer_.add_can2_transmission(0x144, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x145, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x146, std::bit_cast(command)); + transmit_buffer_.trigger_transmission(); + } + + ~right_board() final { + uint16_t command[4] = {0, 0, 0, 0}; + + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(command)); + + transmit_buffer_.add_can2_transmission(0x144, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x145, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x146, std::bit_cast(command)); + transmit_buffer_.trigger_transmission(); + stop_handling_events(); + event_thread_.join(); + } + + void update() { + dr16_.update(); + Steering_Motors[0].update(); + Steering_Motors[1].update(); + Wheel_Motors[0].update(); + Wheel_Motors[1].update(); + for (auto& j : joint) { + j.update_joint(); + } + } + void command() { + uint16_t command[4]; + uint64_t lk_command; + + auto is_arm_enable = *is_arm_enable_; + uint64_t command_; + + int max_count = 100000; + static int counter = 0; + if (counter % 2 == 0) { + + *joint6_error_angle = + is_arm_enable + ? normalizeAngle(joint[2].get_target_theta() - joint[2].get_theta()) + : NAN; + command_ = joint[2].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x146, std::bit_cast(std::bit_cast(uint64_t{command_}))); + command[0] = Wheel_Motors[0].generate_command(); + command[1] = 0; + command[2] = 0; + command[3] = Wheel_Motors[1].generate_command(); + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command)); + } else { + + *joint5_error_angle = + is_arm_enable + ? normalizeAngle(joint[1].get_target_theta() - joint[1].get_theta()) + : NAN; + command_ = joint[1].generate_torque_command(); + + transmit_buffer_.add_can2_transmission( + 0x145, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + *joint4_error_angle = + is_arm_enable + ? normalizeAngle(joint[0].get_target_theta() - joint[0].get_theta()) + : NAN; + command_ = joint[0].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x144, std::bit_cast(std::bit_cast(uint64_t{command_}))); + lk_command = Steering_Motors[0].generate_torque_command(); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(lk_command)); + lk_command = Steering_Motors[1].generate_torque_command(); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(lk_command)); + } + transmit_buffer_.trigger_transmission(); + last_is_arm_enable_ = is_arm_enable; + + counter++; + if (counter >= max_count) { + counter = 0; + } + } + + protected: + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x141) { + Steering_Motors[1].store_status(can_data); + } else if (can_id == 0x142) { + Steering_Motors[0].store_status(can_data); + } else if (can_id == 0x201) { + Wheel_Motors[0].store_status(can_data); + } else if (can_id == 0x204) { + Wheel_Motors[1].store_status(can_data); + } + } + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x146) + joint[2].store_status(can_data); + else if (can_id == 0x145) + joint[1].store_status(can_data); + else if (can_id == 0x144) + joint[0].store_status(can_data); + } + + private: + static double normalizeAngle(double angle) { + if (angle > M_PI) + angle -= 2 * M_PI; + if (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + rclcpp::Logger logger_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + device::Dr16 dr16_; + device::LKMotor Steering_Motors[2]; + device::DjiMotor Wheel_Motors[2]; + device::Joint joint[3]; + bool last_is_arm_enable_ = true; + + InputInterface is_arm_enable_; + OutputInterface joint6_error_angle; + OutputInterface joint5_error_angle; + OutputInterface joint4_error_angle; + } right_board_; + + class left_board final : private librmcs::client::CBoard { + public: + friend class LK_Robot; + explicit left_board( + LK_Robot& LK_Robot, LK_Robot_command& LK_Robot_command, rclcpp::Logger logger, + int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , logger_(std::move(logger)) + + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) + , Steering_Motors( + {LK_Robot, LK_Robot_command, "/steering/steering/lf"}, + {LK_Robot, LK_Robot_command, "/steering/steering/lb"}) + , Wheel_Motors( + {LK_Robot, LK_Robot_command, "/steering/wheel/lf"}, + {LK_Robot, LK_Robot_command, "/steering/wheel/lb"}) + , joint( + {LK_Robot, LK_Robot_command, "/arm/Joint1"}, + {LK_Robot, LK_Robot_command, "/arm/Joint2"}, + {LK_Robot, LK_Robot_command, "/arm/Joint3"}) + , joint2_encoder(LK_Robot, "/arm/Joint2encoder") + , joint3_encoder(LK_Robot, "/arm/Joint3encoder") { + LK_Robot.register_output("/arm/Joint3/control_angle_error", joint3_error_angle); + LK_Robot.register_output("/arm/Joint2/control_angle_error", joint2_error_angle); + LK_Robot.register_output("/arm/Joint1/control_angle_error", joint1_error_angle); + LK_Robot_command.register_input("/arm/enable_flag", is_arm_enable_); + using namespace device; + Wheel_Motors[0].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(11.0)); + Wheel_Motors[1].configure( + device::DjiMotorConfig{device::DjiMotorType::M3508}.set_reduction_ratio(11.0)); + Steering_Motors[0].configure( + device::LKMotorConfig{device::LKMotorType::MHF7015} + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast( + LK_Robot.get_parameter("left_forward_zero_point").as_int())) + .enable_multi_turn_angle()); + Steering_Motors[1].configure( + device::LKMotorConfig{device::LKMotorType::MHF7015} + .set_gear_ratio(1.0) + .set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("left_back_zero_point").as_int())) + .enable_multi_turn_angle()); + joint[2].configure_joint( + LKMotorConfig{LKMotorType::MF7015V210T}, DHConfig{-0.08307, 0, 1.5707963, 0}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint3_qlim").as_double_array()}); + joint[1].configure_joint( + LKMotorConfig{LKMotorType::MF7015V210T}, DHConfig{0.41, 0, 0, 1.5707963}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint2_qlim").as_double_array()}); + joint[0].configure_joint( + device::LKMotorConfig{device::LKMotorType::MG8010E_i36}.set_encoder_zero_point( + LK_Robot.get_parameter("joint1_zero_point").as_int()), + DHConfig{0, 0.05985, 1.5707963, 0}, + Qlim_Stall_Config{LK_Robot.get_parameter("joint1_qlim").as_double_array()}); + joint2_encoder.configure( + EncoderConfig{} + .set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("joint2_zero_point").as_int())) + .enable_multi_turn_angle()); + joint3_encoder.configure( + EncoderConfig{} + .set_encoder_zero_point( + static_cast(LK_Robot.get_parameter("joint3_zero_point").as_int())) + .reverse()); + + uint64_t command; + command = device::LKMotor::lk_quest_command(); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(command)); + + transmit_buffer_.add_can2_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x142, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x143, std::bit_cast(command)); + transmit_buffer_.trigger_transmission(); + } + + ~left_board() final { + uint16_t command[4] = {0, 0, 0, 0}; + + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(command)); + + transmit_buffer_.add_can2_transmission(0x141, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x142, std::bit_cast(command)); + transmit_buffer_.add_can2_transmission(0x143, std::bit_cast(command)); + transmit_buffer_.trigger_transmission(); + stop_handling_events(); + event_thread_.join(); + } + + void update() { + joint2_encoder.update(); + joint3_encoder.update(); + Steering_Motors[0].update(); + Steering_Motors[1].update(); + Wheel_Motors[0].update(); + Wheel_Motors[1].update(); + joint[2].update_joint().change_theta_feedback_(joint3_encoder.get_angle()); + joint[1].update_joint().change_theta_feedback_(joint2_encoder.get_angle()); + joint[0].update_joint(); + } + void command() { + uint16_t command[4]; + uint64_t lk_command; + + auto is_arm_enable = *is_arm_enable_; + uint64_t command_; + + int max_count = 100000; + static int counter = 0; + if (counter % 2 == 0) { + + *joint3_error_angle = + is_arm_enable + ? normalizeAngle(joint[2].get_target_theta() - joint[2].get_theta()) + : NAN; + command_ = joint[2].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x143, std::bit_cast(std::bit_cast(uint64_t{command_}))); + command[0] = 0; + command[1] = Wheel_Motors[1].generate_command(); + command[2] = Wheel_Motors[0].generate_command(); + command[3] = 0; + transmit_buffer_.add_can1_transmission(0x200, std::bit_cast(command)); + + } else { + + *joint2_error_angle = + is_arm_enable + ? -normalizeAngle(joint[1].get_target_theta() - joint[1].get_theta()) + : NAN; + command_ = joint[1].generate_torque_command(); + + transmit_buffer_.add_can2_transmission( + 0x142, std::bit_cast(std::bit_cast(uint64_t{command_}))); + + *joint1_error_angle = + is_arm_enable + ? normalizeAngle(joint[0].get_target_theta() - joint[0].get_theta()) + : NAN; + command_ = joint[0].generate_torque_command(); + transmit_buffer_.add_can2_transmission( + 0x141, std::bit_cast(std::bit_cast(uint64_t{command_}))); + lk_command = Steering_Motors[0].generate_torque_command(); + transmit_buffer_.add_can1_transmission(0x142, std::bit_cast(lk_command)); + lk_command = Steering_Motors[1].generate_torque_command(); + transmit_buffer_.add_can1_transmission(0x141, std::bit_cast(lk_command)); + } + transmit_buffer_.trigger_transmission(); + last_is_arm_enable_ = is_arm_enable; + // RCLCPP_INFO(logger_,"%f",joint[1].get_vel()); + + counter++; + if (counter >= max_count) { + counter = 0; + } + } + + protected: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + // RCLCPP_INFO(logger_, "%x", can_id); + if (can_id == 0x141) { + Steering_Motors[1].store_status(can_data); + } else if (can_id == 0x142) { + Steering_Motors[0].store_status(can_data); + } else if (can_id == 0x203) { + Wheel_Motors[0].store_status(can_data); + } else if (can_id == 0x202) { + Wheel_Motors[1].store_status(can_data); + } + } + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + if (can_id == 0x143) + joint[2].store_status(can_data); + if (can_id == 0x142) + joint[1].store_status(can_data); + if (can_id == 0x141) + joint[0].store_status(can_data); + if (can_id == 0x005) + joint3_encoder.store_status(can_data); + if (can_id == 0x1fb) + joint2_encoder.store_status(can_data); + } + + private: + static double normalizeAngle(double angle) { + if (angle > M_PI) + angle -= 2 * M_PI; + if (angle < -M_PI) + angle += 2 * M_PI; + return angle; + } + rclcpp::Logger logger_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + + device::LKMotor Steering_Motors[2]; + device::DjiMotor Wheel_Motors[2]; + + OutputInterface joint3_error_angle; + OutputInterface joint2_error_angle; + OutputInterface joint1_error_angle; + device::Joint joint[3]; + device::Encoder joint2_encoder; + device::Encoder joint3_encoder; + InputInterface is_arm_enable_; + bool last_is_arm_enable_ = true; + } left_board_; +}; + +} // namespace rmcs_core::hardware +#include +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::LK_Robot, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp new file mode 100644 index 00000000..b387b972 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/cfs_scheduler.hpp @@ -0,0 +1,108 @@ +#pragma once + +#include + +#include + +#include "referee/app/ui/shape/red_black_tree.hpp" + +namespace rmcs_core::referee::app::ui { + +template +class CfsScheduler { +public: + class __attribute__((packed, aligned(sizeof(void*)))) Entity + : private RedBlackTree::Node { + public: + friend class CfsScheduler; + friend class RedBlackTree; + + bool is_in_run_queue() requires(std::is_base_of_v) { + return !RedBlackTree::Node::is_dangling(); + } + + void enter_run_queue(uint16_t priority) requires(std::is_base_of_v) { + if (this->priority_ != priority) { + vruntime_ += this->priority_; + vruntime_ -= priority; + if (this->vruntime_ < min_vruntime_) + this->vruntime_ = min_vruntime_; + + this->priority_ = priority; + + if (is_in_run_queue()) + run_queue_.erase(*this); + } else { + if (is_in_run_queue()) + return; + } + + run_queue_.insert(*this); + } + + void leave_run_queue() requires(std::is_base_of_v) { + if (is_in_run_queue()) [[likely]] + run_queue_.erase(*this); + } + + private: + bool operator<(const Entity& obj) const { return vruntime_ < obj.vruntime_; } + uint64_t vruntime_ : 48 = 65536; + uint16_t priority_ = 0; + }; + + // class T : public Entity {}; + + class UpdateIterator { + public: + UpdateIterator() + : current_(run_queue_.first()) + , ignored_(nullptr) {} + UpdateIterator(const UpdateIterator&) = delete; + UpdateIterator& operator=(const UpdateIterator&) = delete; + UpdateIterator(UpdateIterator&&) = default; + UpdateIterator& operator=(UpdateIterator&&) = default; + + T* get() const { + // NOLINTNEXTLINE(cppcoreguidelines-pro-type-static-cast-downcast) + return static_cast(current_); + } + + T& operator*() const { return *get(); } + T* operator->() const { return get(); } + explicit operator bool() const { return get(); } + + auto update() { + min_vruntime_ = current_->vruntime_; + int shift = 65536 - current_->priority_; + current_->vruntime_ += shift; + + run_queue_.erase(*current_); + auto result = get()->update(); + current_ = ignored_ ? ignored_->next() : run_queue_.first(); + + return result; + } + + void ignore() { + ignored_ = current_; + current_ = ignored_->next(); + } + + private: + Entity *current_, *ignored_; + }; + + static inline bool empty() { return run_queue_.empty(); } + + static inline UpdateIterator get_update_iterator() + requires std::is_base_of_v && requires(T t) { t.update(); } { + return UpdateIterator{}; + } + +private: + static inline RedBlackTree run_queue_; + static inline uint64_t min_vruntime_ = 0; +}; + +} // namespace rmcs_core::referee::app::ui \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp new file mode 100644 index 00000000..b95766a9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/red_black_tree.hpp @@ -0,0 +1,682 @@ +#pragma once + +#include +#include + +#include + +#define WRITE_ONCE(x, val) x = (val) + +class BasicRedBlackTree { +public: + enum class Color : uint8_t { RED = 0, BLACK = 1 }; + + class Node { + public: + friend class BasicRedBlackTree; + + // template + // friend class RedBlackTree; + + Node() = default; + + Color color() const { return static_cast(parent_and_color & 1); } + bool is_red() const { return color() == Color::RED; } + bool is_black() const { return color() == Color::BLACK; } + + Node* parent() const { + return reinterpret_cast(parent_and_color & ~static_cast(1)); + } + // Node* right() const { return right_; } + // Node* left() const { return left_; } + + Node* next() const { + Node *parent, *node = const_cast(this); + + /* + * If we have a right-hand child, go down and then left as far + * as we can. + */ + if (node->right) { + node = node->right; + while (node->left) + node = node->left; + return node; + } + + /* + * No right-hand children. Everything down and left is smaller than us, + * so any 'next' node must be in the general direction of our parent. + * Go up the tree; any time the ancestor is a right-hand child of its + * parent, keep going up. First time it's a left-hand child of its + * parent, said parent is our 'next' node. + */ + while ((parent = node->parent()) && node == parent->right) + node = parent; + + return parent; + } + + Node* prev() const { + Node *parent, *node = const_cast(this); + + /* + * If we have a left-hand child, go down and then right as far + * as we can. + */ + if (node->left) { + node = node->left; + while (node->right) + node = node->right; + return node; + } + + /* + * No left-hand children. Go up till we find an ancestor which + * is a right-hand child of its parent. + */ + while ((parent = node->parent()) && node == parent->left) + node = parent; + + return parent; + } + + void set_parent_and_color(Node* parent, Color color) { + parent_and_color = reinterpret_cast(parent) | static_cast(color); + } + + void set_parent(Node* parent) { set_parent_and_color(parent, color()); } + + void set_red() { parent_and_color &= ~static_cast(1); } + void set_black() { parent_and_color |= static_cast(1); } + void set_color(Color color) { + if (color == Color::RED) + set_red(); + else + set_black(); + } + + Node* red_get_parent() const { return reinterpret_cast(parent_and_color); } + + uintptr_t parent_and_color; + Node* right; + Node* left; + }; + + // void insert_root(Node* node) { + // node->set_parent_and_color(nullptr, Color::BLACK); + // node->left = node->right = nullptr; + + // root = node; + // } + + // void insert_left(Node* parent, Node* child) { + // parent->left = child; + + // child->set_parent_and_color(parent, Color::RED); + // child->left = child->right = nullptr; + + // insert_color(child); + // } + + // void insert_right(Node* parent, Node* child) { + // parent->right = child; + + // child->set_parent_and_color(parent, Color::RED); + // child->left = child->right = nullptr; + + // insert_color(child); + // } + + static inline void link_node(Node* node, Node* parent, Node** link) { + node->set_parent_and_color(parent, Color::RED); + node->left = node->right = nullptr; + + *link = node; + } + + void insert_color(Node* node) { + Node *parent = node->parent(), *gparent, *tmp; + + while (true) { + /* + * Loop invariant: node is red. + */ + if (!parent) [[unlikely]] { + /* + * The inserted node is root. Either this is the + * first node, or we recursed at Case 1 below and + * are no longer violating 4). + */ + node->set_parent_and_color(nullptr, Color::BLACK); + break; + } + + /* + * If there is a black parent, we are done. + * Otherwise, take some corrective action as, + * per 4), we don't want a red root or two + * consecutive red nodes. + */ + if (parent->is_black()) + break; + + gparent = parent->red_get_parent(); + + tmp = gparent->right; + if (parent != tmp) { /* parent == gparent->rb_left */ + if (tmp && tmp->is_red()) { + /* + * Case 1 - node's uncle is red (color flips). + * + * G g + * / \ / \ + * p u --> P U + * / / + * n n + * + * However, since g's parent might be red, and + * 4) does not allow this, we need to recurse + * at g. + */ + tmp->set_parent_and_color(gparent, Color::BLACK); + parent->set_parent_and_color(gparent, Color::BLACK); + node = gparent; + parent = node->parent(); + node->set_parent_and_color(parent, Color::RED); + continue; + } + + tmp = parent->right; + if (node == tmp) { + /* + * Case 2 - node's uncle is black and node is + * the parent's right child (left rotate at parent). + * + * G G + * / \ / \ + * p U --> n U + * \ / + * n p + * + * This still leaves us in violation of 4), the + * continuation into Case 3 will fix that. + */ + tmp = node->left; + WRITE_ONCE(parent->right, tmp); + WRITE_ONCE(node->left, parent); + if (tmp) + tmp->set_parent_and_color(parent, Color::BLACK); + parent->set_parent_and_color(node, Color::RED); + parent = node; + tmp = node->right; + } + + /* + * Case 3 - node's uncle is black and node is + * the parent's left child (right rotate at gparent). + * + * G P + * / \ / \ + * p U --> n g + * / \ + * n U + */ + WRITE_ONCE(gparent->left, tmp); /* == parent->rb_right */ + WRITE_ONCE(parent->right, gparent); + if (tmp) + tmp->set_parent_and_color(gparent, Color::BLACK); + rotate_set_parents(gparent, parent, Color::RED); + break; + } else { + tmp = gparent->left; + if (tmp && tmp->is_red()) { + /* Case 1 - color flips */ + tmp->set_parent_and_color(gparent, Color::BLACK); + parent->set_parent_and_color(gparent, Color::BLACK); + node = gparent; + parent = node->parent(); + node->set_parent_and_color(parent, Color::RED); + continue; + } + + tmp = parent->left; + if (node == tmp) { + /* Case 2 - right rotate at parent */ + tmp = node->right; + WRITE_ONCE(parent->left, tmp); + WRITE_ONCE(node->right, parent); + if (tmp) + tmp->set_parent_and_color(parent, Color::BLACK); + parent->set_parent_and_color(node, Color::RED); + parent = node; + tmp = node->left; + } + + /* Case 3 - left rotate at gparent */ + WRITE_ONCE(gparent->right, tmp); /* == parent->rb_left */ + WRITE_ONCE(parent->left, gparent); + if (tmp) + tmp->set_parent_and_color(gparent, Color::BLACK); + rotate_set_parents(gparent, parent, Color::RED); + break; + } + } + } + + void erase(Node* node) { + Node* rebalance; + rebalance = __erase(node); + if (rebalance) + __erase_color(rebalance); + } + + /* + * This function returns the first node (in sort order) of the tree. + */ + Node* first() const { + Node* n; + + n = root; + if (!n) + return nullptr; + while (n->left) + n = n->left; + return n; + } + + Node* last() const { + Node* n; + + n = root; + if (!n) + return nullptr; + while (n->right) + n = n->right; + return n; + } + + Node* root = nullptr; + +private: + void change_child(Node* old_node, Node* new_node, Node* parent) { + if (parent) { + if (parent->left == old_node) + WRITE_ONCE(parent->left, new_node); + else + WRITE_ONCE(parent->right, new_node); + } else + WRITE_ONCE(root, new_node); + } + + /* + * Helper function for rotations: + * - old's parent and color get assigned to new + * - old gets assigned new as a parent and 'color' as a color. + */ + void rotate_set_parents(Node* old_node, Node* new_node, Color color) { + Node* parent = old_node->parent(); + new_node->parent_and_color = old_node->parent_and_color; + old_node->set_parent_and_color(new_node, color); + change_child(old_node, new_node, parent); + } + + Node* __erase(Node* node) { + Node* child = node->right; + Node* tmp = node->left; + Node *parent, *rebalance; + uintptr_t pc; + + if (!tmp) { + /* + * Case 1: node to erase has no more than 1 child (easy!) + * + * Note that if there is one child it must be red due to 5) + * and node must be black due to 4). We adjust colors locally + * so as to bypass __rb_erase_color() later on. + */ + pc = node->parent_and_color; + parent = ((Node*)(pc & ~3)); + change_child(node, child, parent); + if (child) { + child->parent_and_color = pc; + rebalance = nullptr; + } else + rebalance = ((pc) & 1) ? parent : nullptr; + tmp = parent; + } else if (!child) { + /* Still case 1, but this time the child is node->rb_left */ + tmp->parent_and_color = pc = node->parent_and_color; + parent = ((Node*)(pc & ~3)); + change_child(node, tmp, parent); + rebalance = nullptr; + tmp = parent; + } else { + Node *successor = child, *child2; + + tmp = child->left; + if (!tmp) { + /* + * Case 2: node's successor is its right child + * + * (n) (s) + * / \ / \ + * (x) (s) -> (x) (c) + * \ + * (c) + */ + parent = successor; + child2 = successor->right; + + } else { + /* + * Case 3: node's successor is leftmost under + * node's right child subtree + * + * (n) (s) + * / \ / \ + * (x) (y) -> (x) (y) + * / / + * (p) (p) + * / / + * (s) (c) + * \ + * (c) + */ + do { + parent = successor; + successor = tmp; + tmp = tmp->left; + } while (tmp); + child2 = successor->right; + WRITE_ONCE(parent->left, child2); + WRITE_ONCE(successor->right, child); + child->set_parent(successor); + } + + tmp = node->left; + WRITE_ONCE(successor->left, tmp); + tmp->set_parent(successor); + + pc = node->parent_and_color; + tmp = ((Node*)(pc & ~3)); + change_child(node, successor, tmp); + + if (child2) { + child2->set_parent_and_color(parent, Color::BLACK); + rebalance = nullptr; + } else { + rebalance = successor->is_black() ? parent : nullptr; + } + successor->parent_and_color = pc; + tmp = successor; + } + + return rebalance; + } + + void __erase_color(Node* parent) { + Node *node = nullptr, *sibling, *tmp1, *tmp2; + + while (true) { + /* + * Loop invariants: + * - node is black (or NULL on first iteration) + * - node is not the root (parent is not NULL) + * - All leaf paths going through parent and node have a + * black node count that is 1 lower than other leaf paths. + */ + sibling = parent->right; + if (node != sibling) { /* node == parent->rb_left */ + if (sibling->is_red()) { + /* + * Case 1 - left rotate at parent + * + * P S + * / \ / \ + * N s --> p Sr + * / \ / \ + * Sl Sr N Sl + */ + tmp1 = sibling->left; + WRITE_ONCE(parent->right, tmp1); + WRITE_ONCE(sibling->left, parent); + tmp1->set_parent_and_color(parent, Color::BLACK); + rotate_set_parents(parent, sibling, Color::RED); + sibling = tmp1; + } + tmp1 = sibling->right; + if (!tmp1 || tmp1->is_black()) { + tmp2 = sibling->left; + if (!tmp2 || tmp2->is_black()) { + /* + * Case 2 - sibling color flip + * (p could be either color here) + * + * (p) (p) + * / \ / \ + * N S --> N s + * / \ / \ + * Sl Sr Sl Sr + * + * This leaves us violating 5) which + * can be fixed by flipping p to black + * if it was red, or by recursing at p. + * p is red when coming from Case 1. + */ + sibling->set_parent_and_color(parent, Color::RED); + if (parent->is_red()) + parent->set_black(); + else { + node = parent; + parent = node->parent(); + if (parent) + continue; + } + break; + } + /* + * Case 3 - right rotate at sibling + * (p could be either color here) + * + * (p) (p) + * / \ / \ + * N S --> N sl + * / \ \ + * sl Sr S + * \ + * Sr + * + * Note: p might be red, and then both + * p and sl are red after rotation(which + * breaks property 4). This is fixed in + * Case 4 (in __rb_rotate_set_parents() + * which set sl the color of p + * and set p RB_BLACK) + * + * (p) (sl) + * / \ / \ + * N sl --> P S + * \ / \ + * S N Sr + * \ + * Sr + */ + tmp1 = tmp2->right; + WRITE_ONCE(sibling->left, tmp1); + WRITE_ONCE(tmp2->right, sibling); + WRITE_ONCE(parent->right, tmp2); + if (tmp1) + tmp1->set_parent_and_color(sibling, Color::BLACK); + tmp1 = sibling; + sibling = tmp2; + } + /* + * Case 4 - left rotate at parent + color flips + * (p and sl could be either color here. + * After rotation, p becomes black, s acquires + * p's color, and sl keeps its color) + * + * (p) (s) + * / \ / \ + * N S --> P Sr + * / \ / \ + * (sl) sr N (sl) + */ + tmp2 = sibling->left; + WRITE_ONCE(parent->right, tmp2); + WRITE_ONCE(sibling->left, parent); + tmp1->set_parent_and_color(sibling, Color::BLACK); + if (tmp2) + tmp2->set_parent(parent); + rotate_set_parents(parent, sibling, Color::BLACK); + break; + } else { + sibling = parent->left; + if (sibling->is_red()) { + /* Case 1 - right rotate at parent */ + tmp1 = sibling->right; + WRITE_ONCE(parent->left, tmp1); + WRITE_ONCE(sibling->right, parent); + tmp1->set_parent_and_color(parent, Color::BLACK); + rotate_set_parents(parent, sibling, Color::RED); + sibling = tmp1; + } + tmp1 = sibling->left; + if (!tmp1 || tmp1->is_black()) { + tmp2 = sibling->right; + if (!tmp2 || tmp2->is_black()) { + /* Case 2 - sibling color flip */ + sibling->set_parent_and_color(parent, Color::RED); + if (parent->is_red()) + parent->set_black(); + else { + node = parent; + parent = node->parent(); + if (parent) + continue; + } + break; + } + /* Case 3 - left rotate at sibling */ + tmp1 = tmp2->left; + WRITE_ONCE(sibling->right, tmp1); + WRITE_ONCE(tmp2->left, sibling); + WRITE_ONCE(parent->left, tmp2); + if (tmp1) + tmp1->set_parent_and_color(sibling, Color::BLACK); + tmp1 = sibling; + sibling = tmp2; + } + /* Case 4 - right rotate at parent + color flips */ + tmp2 = sibling->right; + WRITE_ONCE(parent->left, tmp2); + WRITE_ONCE(sibling->right, parent); + tmp1->set_parent_and_color(sibling, Color::BLACK); + if (tmp2) + tmp2->set_parent(parent); + rotate_set_parents(parent, sibling, Color::BLACK); + break; + } + } + } +}; + +template +class RedBlackTree final { +public: + class Node : private BasicRedBlackTree::Node { + public: + friend class RedBlackTree; + Node() { set_dangling(); } + + bool is_dangling() const { return BasicRedBlackTree::Node::parent_and_color == 0; } + + bool is_red() const { return BasicRedBlackTree::Node::is_red(); } + bool is_black() const { return BasicRedBlackTree::Node::is_black(); } + + T* parent() { + return static_cast(static_cast(BasicRedBlackTree::Node::parent())); + } + T* right() { return static_cast(static_cast(BasicRedBlackTree::Node::right)); } + T* left() { return static_cast(static_cast(BasicRedBlackTree::Node::left)); } + + T* next() { return static_cast(static_cast(BasicRedBlackTree::Node::next())); } + T* prev() { return static_cast(static_cast(BasicRedBlackTree::Node::prev())); } + + private: + void set_dangling() { BasicRedBlackTree::Node::parent_and_color = 0; } + }; + + bool insert(T& node) requires(std::is_base_of_v) { + if (!static_cast(node).is_dangling()) + return false; + + BasicRedBlackTree::Node **link = &(tree_.root), *parent = nullptr; + + /* Figure out where to put new node */ + while (*link) { + parent = *link; + + T& current = *static_cast(static_cast(*link)); + if (node < current) + link = &((*link)->left); + else + link = &((*link)->right); + } + + /* Add new node and rebalance tree. */ + tree_.link_node(static_cast(&node), parent, link); + tree_.insert_color(static_cast(&node)); + return true; + } + + bool insert_set(T& node) requires(std::is_base_of_v) { + if (!static_cast(node).is_dangling()) + return false; + + BasicRedBlackTree::Node **link = &(tree_.root), *parent = nullptr; + + /* Figure out where to put new node */ + while (*link) { + parent = *link; + + T& current = *static_cast(static_cast(*link)); + if (node < current) + link = &((*link)->left); + else if (current < node) + link = &((*link)->right); + else + return false; + } + + /* Add new node and rebalance tree. */ + tree_.link_node(static_cast(&node), parent, link); + tree_.insert_color(static_cast(&node)); + return true; + } + + bool erase(T& node) requires(std::is_base_of_v) { + if (static_cast(node).is_dangling()) + return false; + + tree_.erase(static_cast(&node)); + static_cast(node).set_dangling(); + return true; + } + + bool empty() const requires(std::is_base_of_v) { return tree_.root == nullptr; } + + T* root() const requires(std::is_base_of_v) { + return static_cast(static_cast(tree_.root)); + } + T* first() const requires(std::is_base_of_v) { + return static_cast(static_cast(tree_.first())); + } + T* last() const requires(std::is_base_of_v) { + return static_cast(static_cast(tree_.last())); + } + +private: + BasicRedBlackTree tree_; +}; \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp new file mode 100644 index 00000000..7fd0d851 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/remote_shape.hpp @@ -0,0 +1,126 @@ +#pragma once + +#include + +#include "referee/app/ui/shape/red_black_tree.hpp" + +namespace rmcs_core::referee::app::ui { +template +class RemoteShape { +public: + class Descriptor : private RedBlackTree::Node { + public: + friend class RemoteShape; + friend class RedBlackTree; + + Descriptor() = default; + Descriptor(const Descriptor&) = delete; + Descriptor& operator=(const Descriptor&) = delete; + Descriptor(Descriptor&&) = delete; + Descriptor& operator=(Descriptor&& obj) = delete; + + [[nodiscard]] bool has_id() const { return id_; } + [[nodiscard]] bool try_assign_id() + requires std::is_base_of_v && requires(T t) { t.id_revoked(); } { + if (has_id()) [[unlikely]] + return false; + + if (Descriptor* first = swapping_queue_.first()) { + // Optimization: Try to find a descriptor to avoid creating a new one. + swapping_queue_.erase(*first); + swap_id(*first); + return true; + } + + if (next_id_ > id_assignment_max) [[unlikely]] + return false; + else { + assign_id(); + return true; + } + } + [[nodiscard]] bool predict_try_assign_id(uint8_t& existence_confidence) const { + if (has_id()) [[unlikely]] + return false; + + if (Descriptor* first = swapping_queue_.first()) { + existence_confidence = first->existence_confidence_; + return true; + } + + return next_id_ <= id_assignment_max; + } + + [[nodiscard]] bool swapping_enabled() const { + return !RedBlackTree::Node::is_dangling(); + } + void enable_swapping() { + if (swapping_enabled()) + return; + swapping_queue_.insert(*this); + } + void disable_swapping() { + if (!swapping_enabled()) + return; + swapping_queue_.erase(*this); + } + + [[nodiscard]] uint8_t id() const { return id_; } + [[nodiscard]] uint8_t existence_confidence() const { return existence_confidence_; } + + uint8_t increase_existence_confidence() { + ++existence_confidence_; + if (swapping_enabled()) { + disable_swapping(), enable_swapping(); + } + return existence_confidence_; + } + + private: + /* Swap requirement: !this->id_ && victim.id_ */ + void swap_id(Descriptor& victim) { + id_ = victim.id_; + assigned_list_[id_ - 1] = this; + existence_confidence_ = victim.existence_confidence_; + + victim.revoke_id(); + } + + /* Assign requirement: next_id_ <= id_assignment_max */ + void assign_id() { + id_ = next_id_++; + + assigned_list_[id_ - 1] = this; + } + + void revoke_id() { + id_ = 0; + existence_confidence_ = 0; + + static_cast(this)->id_revoked(); + } + + bool operator<(const Descriptor& obj) const { + return existence_confidence_ < obj.existence_confidence_; + } + + uint8_t id_ = 0; + uint8_t existence_confidence_ = 0; + }; + + static inline void force_revoke_all_id() { + for (int i = 0; i < next_id_ - 1; ++i) { + assigned_list_[i]->revoke_id(); + } + next_id_ = 1; + } + +private: + static constexpr uint8_t id_assignment_max = 201; + + static inline uint8_t next_id_ = 1; + static inline Descriptor* assigned_list_[id_assignment_max]; + + static inline RedBlackTree swapping_queue_; +}; +} // namespace rmcs_core::referee::app::ui \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp new file mode 100644 index 00000000..04e7b3a0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/shape/shape.hpp @@ -0,0 +1,779 @@ +#pragma once + +#include +#include +#include +#include + +#include "referee/app/ui/shape/cfs_scheduler.hpp" +#include "referee/app/ui/shape/remote_shape.hpp" +#include "referee/command/field.hpp" + +namespace rmcs_core::referee { + +namespace command::interaction { +class Ui; +} + +namespace app::ui { + +class Shape + : private CfsScheduler::Entity + , private RemoteShape::Descriptor { +public: + friend class CfsScheduler; + friend class RemoteShape; + friend class command::interaction::Ui; + + bool visible() const { return visible_; } + void set_visible(bool value) { + if (visible_ == value) + return; + + visible_ = value; + + // Optimizations + if (!visible_) { + if (existence_confidence() == 0) { + // Simply leave run_queue when shape was hidden and remote shape does not exist. + leave_run_queue(); + return; + } else { + // Otherwise enable swapping + enable_swapping(); + } + } else { + // Disable swapping when shape is visible + disable_swapping(); + } + + sync_confidence_ = 0; + enter_run_queue(); + } + + uint8_t priority() const { return priority_; } + void set_priority(uint8_t value) { + if (priority_ == value) + return; + priority_ = value; + if (is_in_run_queue()) + enter_run_queue(); + } + + uint16_t width() const { return part2_.width; } + void set_width(uint16_t width) { + if (part2_.width == width) + return; + part2_.width = width; + set_modified(); + } + + uint16_t x() const { return part2_.x; } + void set_x(uint16_t x) { + if (part2_.x == x) + return; + part2_.x = x; + set_modified(); + } + + uint16_t y() const { return part2_.y; } + void set_y(uint16_t y) { + if (part2_.y == y) + return; + part2_.y = y; + set_modified(); + } + + bool is_text_shape() const { return is_text_shape_; } + + enum class Operation : uint8_t { + NO_OPERATION = 0, + ADD = 1, + MODIFY = 2, + DELETE = 3, + }; + + Operation predict_update() const { + uint8_t predict_existence = existence_confidence(); + uint8_t predict_sync = sync_confidence_; + + if (!has_id() && !predict_try_assign_id(predict_existence)) { + return Operation::NO_OPERATION; + } + + if (predict_existence == 0) { + predict_sync = max_update_times; + } + + command::Field field; + if (visible_ + && (predict_existence <= predict_sync + || (last_time_modified_ && predict_existence < max_update_times))) { + return Operation::ADD; + } else { + return Operation::MODIFY; + } + } + + constexpr static inline command::Field no_operation_description() { + return command::Field{[](std::byte* buffer) { + auto& description = *new (buffer) DescriptionField{}; + description.part1.operation_type = Operation::NO_OPERATION; + return sizeof(DescriptionField); + }}; + } + + enum class Color : uint8_t { + SELF = 0, + YELLOW = 1, + GREEN = 2, + ORANGE = 3, + PURPLE = 4, + PINK = 5, + CYAN = 6, + BLACK = 7, + WHITE = 8, + }; + +protected: + enum class ShapeType : uint8_t { + LINE = 0, + RECTANGLE = 1, + CIRCLE = 2, + ELLIPSE = 3, + ARC = 4, + FLOAT = 5, + INTEGER = 6, + TEXT = 7, + }; + + struct DescriptionField { + uint8_t name[3]; + struct __attribute__((packed)) Part1 { + Operation operation_type : 3; + ShapeType shape_type : 3; + uint8_t layer : 4; + Color color : 4; + uint16_t details_a : 9; + uint16_t details_b : 9; + } part1; + struct __attribute__((packed)) Part2 { + uint16_t width : 10; + uint16_t x : 11; + uint16_t y : 11; + } part2; + struct __attribute__((packed)) Part3 { + uint16_t details_c : 10; + uint16_t details_d : 11; + uint16_t details_e : 11; + } part3; + }; + + void set_modified() { + // Optimization: Assume the modification not exist when invisible. + if (!visible_) + return; + + sync_confidence_ = 0; + enter_run_queue(); + } + + virtual size_t write_description_field(std::byte* buffer) = 0; + + DescriptionField::Part2 part2_ alignas(4); + +private: + void enter_run_queue() { + uint8_t min_confidence = std::min(existence_confidence(), sync_confidence_); + uint16_t weighted_priority = (priority_ - 256) << (4 * min_confidence); + CfsScheduler::Entity::enter_run_queue(weighted_priority); + } + + void id_revoked() { + // This is a callback indicating that the remote id that this shape once had + // is no longer associated with it. + // Called by RemoteShape::Descriptor. + if (visible_) { + // Re-enter the update queue to try to get a new id. + set_modified(); + } else { + // Leave run_queue when shape was hidden. + leave_run_queue(); + } + } + + command::Field update() { + // This is a callback indicating that the shape is being updated. + // Called by CfsScheduler. + + if (!has_id() && !try_assign_id()) { + // TODO: Print error message. + sync_confidence_ = max_update_times; + visible_ = false; + // Do nothing when failed + return no_operation_description(); + } + + if (existence_confidence() == 0) { + // Optimization: Always consider it synchronized when remote shape does not exist. + sync_confidence_ = max_update_times; + } + + command::Field field; + + // Optimization1: Stop adding when shape is invisible. + // Optimization2: Prevent continuous modification. + if (visible_ + && (existence_confidence() <= sync_confidence_ + || (last_time_modified_ && existence_confidence() < max_update_times))) { + // Send add packet + last_time_modified_ = false; + field = command::Field{[this](std::byte* buffer) { + return write_full_description_field(buffer, Operation::ADD); + }}; + if (increase_existence_confidence() < max_update_times + || sync_confidence_ < max_update_times) + enter_run_queue(); + } else { + // Send modify packet + last_time_modified_ = true; + field = command::Field{[this](std::byte* buffer) { + return write_full_description_field(buffer, Operation::MODIFY); + }}; + // No need to compare existence_confidence here. + // Because either the shape is not visible here, no need to send add packet. + // Or existence_confidence > sync_confidence, only the min value needs to be considered. + if (++sync_confidence_ < max_update_times) + enter_run_queue(); + } + + return field; + } + + size_t write_full_description_field(std::byte* buffer, Operation operation) { + size_t written = + visible_ ? write_description_field(buffer) : write_invisible_description_field(buffer); + auto& description = *std::launder(reinterpret_cast(buffer)); + + // No special meaning, just to ensure no duplication + description.name[0] = id(); + description.name[1] = 0xef; + description.name[2] = 0xfe; + + // We only use layer 0 + description.part1.layer = 0; + + description.part1.operation_type = operation; + + return written; + } + + static inline size_t write_invisible_description_field(std::byte* buffer) { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::LINE; + description.part1.color = Color::WHITE; + + description.part2.width = 0; + description.part2.x = 0; + description.part2.y = 0; + + description.part3.details_c = 0; + description.part3.details_d = 0; + description.part3.details_e = 0; + + return sizeof(DescriptionField); + } + + static constexpr uint8_t max_update_times = 4; + + uint8_t priority_ = 15; + uint8_t sync_confidence_ : 5 = max_update_times; + bool is_text_shape_ : 1 = false; + bool last_time_modified_ : 1 = false; + bool visible_ : 1 = false; +}; + +class Line : public Shape { +public: + Line() = default; + Line( + Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t x2, uint16_t y2, + bool visible = true) { + part3_.color = color; + part2_.width = width; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; + + set_visible(visible); + } + + Color color() const { return part3_.color; } + void set_color(Color color) { + if (part3_.color == color) + return; + part3_.color = color; + set_modified(); + } + + uint16_t x2() const { return part3_.x2; } + void set_x2(uint16_t x2) { + if (part3_.x2 == x2) + return; + part3_.x2 = x2; + set_modified(); + } + + uint16_t y2() const { return part3_.y2; } + void set_y2(uint16_t y2) { + if (part3_.y2 == y2) + return; + part3_.y2 = y2; + set_modified(); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::LINE; + description.part1.color = part3_.color; + + description.part2 = part2_; + + description.part3 = std::bit_cast(part3_); + + return sizeof(DescriptionField); + } + +private: + struct __attribute__((packed)) { + // Since details_c is invalid in lines, the memory is used to store color + Color color : 8; + uint8_t placeholder : 2; + + uint16_t x2 : 11; + uint16_t y2 : 11; + } part3_ alignas(4); +}; + +class Circle : public Shape { +public: + Circle() = default; + Circle( + Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t rx, uint16_t ry, + bool visible = true) { + part3_.color = color; + part2_.width = width; + part2_.x = x; + part2_.y = y; + part3_.rx = rx; + part3_.ry = ry; + + set_visible(visible); + } + + Color color() const { return part3_.color; } + void set_color(Color color) { + if (part3_.color == color) + return; + part3_.color = color; + set_modified(); + } + + void set_r(uint16_t r) { + set_rx(r); + set_ry(r); + } + + uint16_t rx() const { return part3_.rx; } + void set_rx(uint16_t rx) { + if (part3_.rx == rx) + return; + part3_.rx = rx; + set_modified(); + } + + uint16_t ry() const { return part3_.ry; } + void set_ry(uint16_t ry) { + if (part3_.ry == ry) + return; + part3_.ry = ry; + set_modified(); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::ELLIPSE; + description.part1.color = part3_.color; + + description.part2 = part2_; + + description.part3 = std::bit_cast(part3_); + + return sizeof(DescriptionField); + } + + struct __attribute__((packed)) { + Color color : 8; + uint8_t placeholder : 2; + + uint16_t rx : 11; + uint16_t ry : 11; + } part3_ alignas(4); +}; + +class Rectangle : public Shape { +public: + Rectangle() = default; + Rectangle( + Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t x2, uint16_t y2, + bool visible = true) { + part3_.color = color; + part2_.width = width; + part2_.x = x; + part2_.y = y; + part3_.x2 = x2; + part3_.y2 = y2; + + set_visible(visible); + } + + Color color() const { return part3_.color; } + void set_color(Color color) { + if (part3_.color == color) + return; + part3_.color = color; + set_modified(); + } + + uint16_t x2() const { return part3_.x2; } + void set_x2(uint16_t x2) { + if (part3_.x2 == x2) + return; + part3_.x2 = x2; + set_modified(); + } + + uint16_t y2() const { return part3_.y2; } + void set_y2(uint16_t y2) { + if (part3_.y2 == y2) + return; + part3_.y2 = y2; + set_modified(); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::RECTANGLE; + description.part1.color = part3_.color; + + description.part2 = part2_; + + description.part3 = std::bit_cast(part3_); + + return sizeof(DescriptionField); + } + + struct __attribute__((packed)) { + Color color : 8; + uint8_t placeholder : 2; + + uint16_t x2 : 11; + uint16_t y2 : 11; + } part3_ alignas(4); +}; + +class Arc : public Shape { +public: + Arc() = default; + Arc(Color color, uint16_t width, uint16_t x, uint16_t y, uint16_t angle_start, + uint16_t angle_end, uint16_t rx, uint16_t ry, bool visible = true) + : Arc() { + angle_start_ = angle_start; + angle_end_ = angle_end; + + part2_.width = width; + part2_.x = x; + part2_.y = y; + + part3_.color = color; + part3_.rx = rx; + part3_.ry = ry; + + set_visible(visible); + } + + Color color() const { return part3_.color; } + void set_color(Color color) { + if (part3_.color == color) + return; + part3_.color = color; + set_modified(); + } + + uint16_t angle_start() const { return angle_start_; } + void set_angle_start(uint16_t angle_start) { + if (angle_start_ == angle_start) + return; + angle_start_ = angle_start; + set_modified(); + } + + uint16_t angle_end() const { return angle_end_; } + void set_angle_end(uint16_t angle_end) { + if (angle_end_ == angle_end) + return; + angle_end_ = angle_end; + set_modified(); + } + + void set_angle(uint16_t midpoint, uint16_t half_central_angle) { + int start = midpoint - half_central_angle; + if (start < 0) + start += 360; + angle_start_ = start; + + int end = midpoint + half_central_angle; + if (end >= 360) + end -= 360; + angle_end_ = end; + + set_modified(); + } + + uint16_t rx() const { return part3_.rx; } + void set_rx(uint16_t rx) { + if (part3_.rx == rx) + return; + part3_.rx = rx; + set_modified(); + } + + uint16_t ry() const { return part3_.ry; } + void set_ry(uint16_t ry) { + if (part3_.ry == ry) + return; + part3_.ry = ry; + set_modified(); + } + + void set_r(uint16_t r) { + set_rx(r); + set_ry(r); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::ARC; + description.part1.color = part3_.color; + description.part1.details_a = angle_start_; + description.part1.details_b = angle_end_; + + description.part2 = part2_; + + description.part3 = std::bit_cast(part3_); + + return sizeof(DescriptionField); + } + + uint16_t angle_start_, angle_end_; + + struct __attribute__((packed)) { + Color color : 8; + uint8_t placeholder : 2; + + uint16_t rx : 11; + uint16_t ry : 11; + } part3_ alignas(4); +}; + +class Integer : public Shape { +public: + Integer() = default; + Integer( + Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, int32_t value, + bool visible = true) + : Integer() { + color_ = color; + font_size_ = font_size; + + part2_.width = width; + part2_.x = x; + part2_.y = y; + + value_ = value; + + set_visible(visible); + } + + Color color() const { return color_; } + void set_color(Color color) { + if (color_ == color) + return; + color_ = color; + set_modified(); + } + + void set_center_x(uint16_t x) { + int value = value_; + int number_of_digits = value <= 0 ? 1 : 0; + for (; value != 0; number_of_digits++) + value /= 10; + part2_.x = x - font_size_ * number_of_digits / 2 + font_size_ / 5; + set_modified(); + } + + int32_t value() const { return value_; } + void set_value(int32_t value) { + if (value_ == value) + return; + value_ = value; + set_modified(); + } + + uint16_t font_size() const { return font_size_; } + void set_font_size(uint16_t font_size) { + if (font_size_ == font_size) + return; + font_size_ = font_size; + set_modified(); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::INTEGER; + description.part1.color = color_; + description.part1.details_a = font_size_; + + description.part2 = part2_; + + description.part3 = std::bit_cast(value_); + + return sizeof(DescriptionField); + } + + uint16_t font_size_; + Color color_; + int32_t value_; +}; + +class Float : public Integer { +public: + using Integer::Integer; + + void set_center_x(uint16_t x) { + int value = value_; + int number_of_digits = value < 0 ? 1 : 0; + + int integer_part = value / 1000; + if (integer_part == 0) + ++number_of_digits; + for (; integer_part != 0; number_of_digits++) + integer_part /= 10; + + int decimal_part = value % 1000; + number_of_digits += + 2 * (decimal_part != 0) + (decimal_part % 100 != 0) + (decimal_part % 10 != 0); + part2_.x = x - font_size_ * number_of_digits / 2 + font_size_ / 5; + + set_modified(); + } + + using Integer::set_value; + void set_value(double value) { Integer::set_value(static_cast(std::round(value * 1000))); } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::FLOAT; + description.part1.color = color_; + description.part1.details_a = font_size_; + + description.part2 = part2_; + + description.part3 = std::bit_cast(value_); + + return sizeof(DescriptionField); + } +}; + +class Text : public Shape { +public: + Text() { value_ = nullptr; }; + Text( + Color color, uint16_t font_size, uint16_t width, uint16_t x, uint16_t y, const char* value, + bool visible = true) + : Text() { + color_ = color; + font_size_ = font_size; + + part2_.width = width; + part2_.x = x; + part2_.y = y; + + value_ = value; + + set_visible(visible); + } + + Color color() const { return color_; } + void set_color(Color color) { + if (color_ == color) + return; + color_ = color; + set_modified(); + } + + const char* value() const { return value_; } + void set_value(const char* value) { + if (value_ == value) + return; + value_ = value; + set_modified(); + } + + uint16_t font_size() const { return font_size_; } + void set_font_size(uint16_t font_size) { + if (font_size_ == font_size) + return; + font_size_ = font_size; + set_modified(); + } + +protected: + size_t write_description_field(std::byte* buffer) override { + auto& description = *new (buffer) DescriptionField{}; + + description.part1.shape_type = ShapeType::TEXT; + description.part1.color = color_; + description.part1.details_a = font_size_; + + description.part2 = part2_; + + constexpr size_t data_part_size = 30; + auto str_length = std::min(strlen(value_), data_part_size); + description.part1.details_b = str_length; + std::memcpy(buffer + sizeof(DescriptionField), value_, str_length); + + return sizeof(DescriptionField) + data_part_size; + } + + uint16_t font_size_; + Color color_; + const char* value_; +}; + +} // namespace app::ui +} // namespace rmcs_core::referee \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair.hpp new file mode 100644 index 00000000..8be151b6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/crosshair.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { + +class CrossHair { +public: + CrossHair(Shape::Color color, uint16_t x, uint16_t y, bool visible = true) + : guidelines_( + {color, 2, (uint16_t)(x - r2), y, (uint16_t)(x - r1), y, visible}, + {color, 2, (uint16_t)(x + r1), y, (uint16_t)(x + r2), y, visible}, + {color, 2, x, (uint16_t)(y + r2), x, (uint16_t)(y + r1), visible}, + {color, 2, x, (uint16_t)(y - r1), x, (uint16_t)(y - r2), visible}) + , center_(color, 2, x, y, 1, 1) {} + + void set_visible(bool value) { + for (auto& line : guidelines_) + line.set_visible(value); + center_.set_visible(value); + } + +private: + static constexpr uint16_t r1 = 8, r2 = 24; + + Line guidelines_[4]; + Circle center_; +}; + +} // namespace rmcs_core::referee::app::ui \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp new file mode 100644 index 00000000..4b14f6dd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/app/ui/widget/status_ring.hpp @@ -0,0 +1,330 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include + +#include "referee/app/ui/shape/shape.hpp" + +namespace rmcs_core::referee::app::ui { + +class StatusRing { +public: + StatusRing() { + supercap_status_.set_x(x_center); + supercap_status_.set_y(y_center); + supercap_status_.set_r(visible_radius - width_ring); + supercap_status_.set_angle_start(275); + supercap_status_.set_angle_end(275 + visible_angle); + supercap_status_.set_width(width_ring); + supercap_status_.set_color(Shape::Color::PINK); + supercap_status_.set_visible(true); + + battery_status_.set_x(x_center); + battery_status_.set_y(y_center); + battery_status_.set_r(visible_radius - width_ring); + battery_status_.set_angle_start(265 - visible_angle); + battery_status_.set_angle_end(265); + battery_status_.set_width(width_ring); + battery_status_.set_color(Shape::Color::PINK); + battery_status_.set_visible(true); + + friction_wheel_speed_.set_x(x_center); + friction_wheel_speed_.set_y(y_center); + friction_wheel_speed_.set_r(visible_radius - width_ring); + friction_wheel_speed_.set_angle_start(85 - visible_angle); + friction_wheel_speed_.set_angle_end(85); + friction_wheel_speed_.set_width(width_ring); + friction_wheel_speed_.set_color(Shape::Color::PINK); + friction_wheel_speed_.set_visible(true); + + bullet_status_.set_x(x_center); + bullet_status_.set_y(y_center); + bullet_status_.set_r(visible_radius - width_ring); + bullet_status_.set_angle_start(95); + bullet_status_.set_angle_end(95 + visible_angle); + bullet_status_.set_width(width_ring); + bullet_status_.set_color(Shape::Color::PINK); + bullet_status_.set_visible(true); + + // UI + line_left_center_.set_x(x_center - visible_radius); + line_left_center_.set_x2(x_center - visible_radius + width_ring + 10); + line_left_center_.set_y(y_center); + line_left_center_.set_y2(y_center); + line_left_center_.set_width(40); + line_left_center_.set_color(Shape::Color::WHITE); + line_left_center_.set_visible(true); + + line_right_center_.set_x(x_center + visible_radius); + line_right_center_.set_x2(x_center + visible_radius - width_ring - 10); + line_right_center_.set_y(y_center); + line_right_center_.set_y2(y_center); + line_right_center_.set_width(40); + line_right_center_.set_color(Shape::Color::WHITE); + line_right_center_.set_visible(true); + + arc_left_up_.set_x(x_center); + arc_left_up_.set_y(y_center); + arc_left_up_.set_r(visible_radius - width_ring); + arc_left_up_.set_angle_start(275 + visible_angle + 1); + arc_left_up_.set_angle_end(275 + visible_angle + 3); + arc_left_up_.set_width(width_ring + 10); + arc_left_up_.set_color(Shape::Color::WHITE); + arc_left_up_.set_visible(true); + + arc_left_down_.set_x(x_center); + arc_left_down_.set_y(y_center); + arc_left_down_.set_r(visible_radius - width_ring); + arc_left_down_.set_angle_start(265 - visible_angle - 3); + arc_left_down_.set_angle_end(265 - visible_angle - 1); + arc_left_down_.set_width(width_ring + 10); + arc_left_down_.set_color(Shape::Color::WHITE); + arc_left_down_.set_visible(true); + + arc_right_up_.set_x(x_center); + arc_right_up_.set_y(y_center); + arc_right_up_.set_r(visible_radius - width_ring); + arc_right_up_.set_angle_start(85 - visible_angle - 3); + arc_right_up_.set_angle_end(85 - visible_angle - 1); + arc_right_up_.set_width(width_ring + 10); + arc_right_up_.set_color(Shape::Color::WHITE); + arc_right_up_.set_visible(true); + + arc_right_down_.set_x(x_center); + arc_right_down_.set_y(y_center); + arc_right_down_.set_r(visible_radius - width_ring); + arc_right_down_.set_angle_start(95 + visible_angle + 1); + arc_right_down_.set_angle_end(95 + visible_angle + 3); + arc_right_down_.set_width(width_ring + 10); + arc_right_down_.set_color(Shape::Color::WHITE); + arc_right_down_.set_visible(true); + + // Integer + supercap_voltage_.set_color(Shape::Color::WHITE); + supercap_voltage_.set_font_size(15); + supercap_voltage_.set_width(2); + supercap_voltage_.set_x(x_center - 200); + supercap_voltage_.set_y(y_center + 25); + supercap_voltage_.set_value(0); + supercap_voltage_.set_visible(true); + + battery_voltage_.set_color(Shape::Color::WHITE); + battery_voltage_.set_font_size(15); + battery_voltage_.set_width(2); + battery_voltage_.set_x(x_center - 200); + battery_voltage_.set_y(y_center - 10); + battery_voltage_.set_value(0); + battery_voltage_.set_visible(true); + + bullet_allowance_.set_color(Shape::Color::WHITE); + bullet_allowance_.set_font_size(15); + bullet_allowance_.set_width(2); + bullet_allowance_.set_x(x_center + 100); + bullet_allowance_.set_y(y_center - 10); + bullet_allowance_.set_value(0); + bullet_allowance_.set_visible(true); + + set_limits(26.5, 26.5, 800, 300); + } + + void update_auto_aim_enable(bool enable) { + static bool enable_last_{false}; + + if (enable == enable_last_) + return; + + if (enable) { + arc_left_up_.set_color(Shape::Color::GREEN); + arc_left_down_.set_color(Shape::Color::GREEN); + arc_right_up_.set_color(Shape::Color::GREEN); + arc_right_down_.set_color(Shape::Color::GREEN); + + arc_left_up_.set_width(width_ring + 50); + arc_left_down_.set_width(width_ring + 50); + arc_right_up_.set_width(width_ring + 50); + arc_right_down_.set_width(width_ring + 50); + + arc_left_up_.set_angle_start(275 + visible_angle + 1); + arc_left_up_.set_angle_end(275 + visible_angle + 2); + arc_left_down_.set_angle_start(265 - visible_angle - 2); + arc_left_down_.set_angle_end(265 - visible_angle - 1); + arc_right_up_.set_angle_start(85 - visible_angle - 2); + arc_right_up_.set_angle_end(85 - visible_angle - 1); + arc_right_down_.set_angle_start(95 + visible_angle + 1); + arc_right_down_.set_angle_end(95 + visible_angle + 2); + + } else { + arc_left_up_.set_color(Shape::Color::WHITE); + arc_left_down_.set_color(Shape::Color::WHITE); + arc_right_up_.set_color(Shape::Color::WHITE); + arc_right_down_.set_color(Shape::Color::WHITE); + + arc_left_up_.set_width(width_ring + 10); + arc_left_down_.set_width(width_ring + 10); + arc_right_up_.set_width(width_ring + 10); + arc_right_down_.set_width(width_ring + 10); + + arc_left_up_.set_angle_start(275 + visible_angle + 1); + arc_left_up_.set_angle_end(275 + visible_angle + 3); + arc_left_down_.set_angle_start(265 - visible_angle - 3); + arc_left_down_.set_angle_end(265 - visible_angle - 1); + arc_right_up_.set_angle_start(85 - visible_angle - 3); + arc_right_up_.set_angle_end(85 - visible_angle - 1); + arc_right_down_.set_angle_start(95 + visible_angle + 1); + arc_right_down_.set_angle_end(95 + visible_angle + 3); + } + + enable_last_ = enable; + } + + void update_supercap(double value, bool enable) { + supercap_voltage_.set_value(value); + + auto angle = 275 + calculate_angle(value, 10.5, supercap_limit_) + 1; + supercap_status_.set_angle_end(static_cast(angle)); + + if (value > 22.6) { + supercap_status_.set_color(enable ? Shape::Color::CYAN : Shape::Color::GREEN); + } else if (value > 13.5) { + supercap_status_.set_color(enable ? Shape::Color::YELLOW : Shape::Color::ORANGE); + } else { + supercap_status_.set_color(enable ? Shape::Color::PURPLE : Shape::Color::PINK); + } + } + + void update_battery_power(double value) { + battery_voltage_.set_value(value); + + auto angle = 265 - calculate_angle(value, 20, 25.7) - 1; + battery_status_.set_angle_start(static_cast(angle)); + + if (value > 23.18) { + battery_status_.set_color(Shape::Color::GREEN); + } else if (value > 22.05) { + battery_status_.set_color(Shape::Color::YELLOW); + } else if (value > 21.40) { + battery_status_.set_color(Shape::Color::ORANGE); + } else { + battery_status_.set_color(Shape::Color::PINK); + } + } + + void update_friction_wheel_speed(double value, bool enable) { + auto angle = 85 - calculate_angle(value, 0, friction_limit_) - 1; + friction_wheel_speed_.set_angle_start(static_cast(angle)); + + if (enable) { + friction_wheel_speed_.set_color(Shape::Color::GREEN); + } else { + friction_wheel_speed_.set_color(Shape::Color::PINK); + } + } + + void update_bullet_allowance(uint16_t value) { + auto allowance = std::bit_cast(value); + + // real number + bullet_allowance_.set_value(allowance); + + // limit ring + auto angle = 95 + calculate_angle(allowance, 0, bullet_limit_) + 1; + bullet_status_.set_angle_end(static_cast(angle)); + + if (allowance < 25) { + bullet_status_.set_color(Shape::Color::PINK); + } else if (allowance < 50) { + bullet_status_.set_color(Shape::Color::ORANGE); + } else { + bullet_status_.set_color(Shape::Color::GREEN); + } + } + +private: + static constexpr double calculate_angle(double value, double min, double max) { + return visible_angle * std::clamp(value - min, 0.0, max - min) / (max - min); + } + + void set_limits( + double supercap_limit, double battery_limit, double friction_limit, int16_t bullet_limit) { + supercap_limit_ = supercap_limit; + battery_limit_ = battery_limit; + friction_limit_ = friction_limit; + bullet_limit_ = bullet_limit; + + int scale_angle = 5; + for (auto& bullet_scale : bullet_scales_) { + + scale_angle += (visible_angle) / 4; + + bullet_scale.set_x(x_center); + bullet_scale.set_y(y_center); + bullet_scale.set_r(visible_radius - width_ring - 30); + bullet_scale.set_angle_start(90 + scale_angle - 1); + bullet_scale.set_angle_end(90 + scale_angle); + bullet_scale.set_width(width_ring + 10); + bullet_scale.set_color(Shape::Color::WHITE); + bullet_scale.set_visible(true); + } + + double value = 0; + scale_angle = 5; + for (auto& number : bullet_scales_number_) { + + scale_angle += (visible_angle) / 4; + value += static_cast(bullet_limit_) / 4; + + const auto r = visible_radius - width_ring + 30; + const auto angle = static_cast(-scale_angle) * std::numbers::pi / 180; + + number.set_x(x_center + static_cast(r * std::cos(angle))); + number.set_y(y_center + static_cast(r * std::sin(angle))); + number.set_color(Shape::Color::WHITE); + number.set_font_size(15); + number.set_width(2); + number.set_value(static_cast(value)); + number.set_visible(true); + } + } + + constexpr static uint16_t x_center = 960; + constexpr static uint16_t y_center = 540; + constexpr static uint16_t width_ring = 15; + constexpr static uint16_t visible_radius = 400; + constexpr static uint16_t visible_angle = 40; + + double supercap_limit_; + double battery_limit_; + double friction_limit_; + int16_t bullet_limit_; + + // Dynamic part + Arc supercap_status_; + Float supercap_voltage_; + + Arc battery_status_; + Float battery_voltage_; + + Arc friction_wheel_speed_; + + Arc bullet_status_; + Integer bullet_allowance_; + + // Static part + Line line_left_center_; + Line line_right_center_; + Arc arc_left_up_; + Arc arc_left_down_; + Arc arc_right_up_; + Arc arc_right_down_; + Integer bullet_scales_number_[4]; + Arc bullet_scales_[4]; +}; + +} // namespace rmcs_core::referee::app::ui \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command.cpp b/rmcs_ws/src/rmcs_core/src/referee/command.cpp new file mode 100644 index 00000000..95326d34 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/command.cpp @@ -0,0 +1,106 @@ +#include + +#include +#include +#include +#include + +#include "referee/command/field.hpp" +#include "referee/frame.hpp" + +namespace rmcs_core::referee { +using namespace command; + +class Command + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Command() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , next_sent_(std::chrono::steady_clock::time_point::min()) + , interaction_next_sent_(std::chrono::steady_clock::time_point::min()) + , map_marker_next_sent_(std::chrono::steady_clock::time_point::min()) + , text_display_next_sent_(std::chrono::steady_clock::time_point::min()) { + + register_input("/referee/serial", serial_, false); + + register_input("/referee/command/interaction", interaction_field_, false); + register_input("/referee/command/map_marker", map_marker_field_, false); + register_input("/referee/command/text_display", text_display_field_, false); + } + + void before_updating() override { + if (!interaction_field_.ready()) + interaction_field_.bind_directly(empty_field_); + if (!map_marker_field_.ready()) + map_marker_field_.bind_directly(empty_field_); + if (!text_display_field_.ready()) + text_display_field_.bind_directly(empty_field_); + } + + void update() override { + if (!serial_.ready()) + return; + + using namespace std::chrono_literals; + auto now = std::chrono::steady_clock::now(); + auto& serial = const_cast(*serial_); + + if (now < next_sent_) + return; + + constexpr auto one_second = std::chrono::steady_clock::duration(1s); + size_t data_length; + if (now >= interaction_next_sent_ && !interaction_field_->empty()) { + interaction_next_sent_ = now + (one_second / 25); // 25hz max to reduce packet loss + frame_.body.command_id = 0x0301; + data_length = interaction_field_->write(frame_.body.data); + } else if (now >= map_marker_next_sent_ && !map_marker_field_->empty()) { + map_marker_next_sent_ = now + (one_second / 1); // 1hz max + frame_.body.command_id = 0x0307; + data_length = map_marker_field_->write(frame_.body.data); + } else if (now >= text_display_next_sent_ && !text_display_field_->empty()) { + text_display_next_sent_ = now + (one_second / 3); // 3hz max + frame_.body.command_id = 0x0308; + data_length = text_display_field_->write(frame_.body.data); + } else { + return; + } + + // TODO(qzh): Assert data length. + + frame_.header.sof = sof_value; + frame_.header.data_length = data_length; + frame_.header.sequence = 0; + rmcs_utility::dji_crc::append_crc8(frame_.header); + + auto frame_size = + sizeof(frame_.header) + sizeof(frame_.body.command_id) + data_length + sizeof(uint16_t); + rmcs_utility::dji_crc::append_crc16(&frame_, frame_size); + + serial.write(reinterpret_cast(&frame_), frame_size); + next_sent_ = now + (one_second / 3720 * frame_size); + } + +private: + InputInterface serial_; + Frame frame_; + + Field empty_field_; + std::chrono::steady_clock::time_point next_sent_; + + InputInterface interaction_field_; + std::chrono::steady_clock::time_point interaction_next_sent_; + + InputInterface map_marker_field_; + std::chrono::steady_clock::time_point map_marker_next_sent_; + + InputInterface text_display_field_; + std::chrono::steady_clock::time_point text_display_next_sent_; +}; + +} // namespace rmcs_core::referee + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Command, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/command/field.hpp new file mode 100644 index 00000000..9f511717 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/command/field.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace rmcs_core::referee::command { + +class Field { +public: + constexpr Field() { write_ = &do_nothing; } + + template + requires requires(const F& f, std::byte* buffer) { + { f(buffer) } -> std::convertible_to; + } constexpr explicit Field(F functor) { + static_assert(sizeof(functor) <= sizeof(intptr_t)); + static_assert(std::is_trivially_destructible_v); + + ::new (&storage_) F(std::move(functor)); + write_ = [](const intptr_t& storage, std::byte* buffer) { + return (*std::launder(reinterpret_cast(&storage)))(buffer); + }; + } + + [[nodiscard]] constexpr explicit operator bool() const { return write_ != &do_nothing; } + [[nodiscard]] constexpr bool empty() const { return write_ == &do_nothing; } + + size_t write(std::byte* buffer) const { return write_(storage_, buffer); }; + +private: + static constexpr size_t do_nothing(const intptr_t&, std::byte*) { return 0; }; + + intptr_t storage_; + size_t (*write_)(const intptr_t&, std::byte*); +}; + +inline size_t write_field(std::byte*) { return 0; } + +template +inline size_t write_field(std::byte*, const Field&, const Ts&...); +template +inline size_t write_field(std::byte*, const T&, const Ts&...); + +template +inline size_t write_field(std::byte* buffer, const Field& package, const Ts&... other_data) { + auto written = package.write(buffer); + return written + write_field(buffer + written, other_data...); +} +template +inline size_t write_field(std::byte* buffer, const T& data, const Ts&... other_data) { + memcpy(buffer, &data, sizeof(data)); + return sizeof(data) + write_field(buffer + sizeof(data), other_data...); +} + +#define MAKE_FIELD(...) \ + ::rmcs_core::referee::command::Field { \ + [this](std::byte* buffer) { return write_field(buffer, __VA_ARGS__); } \ + } + +} // namespace rmcs_core::referee::command \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction.cpp new file mode 100644 index 00000000..a590fdb8 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction.cpp @@ -0,0 +1,65 @@ +#include +#include + +#include "referee/command/field.hpp" + +namespace rmcs_core::referee::command { + +class Interaction + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Interaction() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + register_input( + "/referee/command/interaction/sentry_decision", sentry_decision_field_, false); + register_input("/referee/command/interaction/communicate", communicate_field_, false); + register_input("/referee/command/interaction/ui", ui_field_, false); + + register_output("/referee/command/interaction", interaction_field_); + } + + void before_updating() override { + if (!sentry_decision_field_.ready()) + sentry_decision_field_.bind_directly(empty_field_); + if (!communicate_field_.ready()) + communicate_field_.bind_directly(empty_field_); + if (!ui_field_.ready()) + ui_field_.bind_directly(empty_field_); + } + + void update() override { + if (*sentry_decision_field_) + *interaction_field_ = *sentry_decision_field_; + else if (*communicate_field_) + *interaction_field_ = *communicate_field_; + else if (*ui_field_) + *interaction_field_ = *ui_field_; + else + *interaction_field_ = Field{}; + } + +private: + Field empty_field_; + + InputInterface sentry_decision_field_; + InputInterface communicate_field_; + InputInterface ui_field_; + + struct __attribute__((packed)) InteractionHeader { + uint16_t command_id; + uint16_t sender_id; + uint16_t receiver_id; + }; + + OutputInterface interaction_field_; +}; + +} // namespace rmcs_core::referee::command + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::Interaction, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/communicate.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/communicate.cpp new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp new file mode 100644 index 00000000..97223d51 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/header.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +#include + +namespace rmcs_core::referee::command::interaction { + +struct __attribute__((packed)) Header { + uint16_t command_id; + uint16_t sender_id; + uint16_t receiver_id; +}; + +} // namespace rmcs_core::referee::command::interaction \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp new file mode 100644 index 00000000..dc1e3271 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/ui.cpp @@ -0,0 +1,168 @@ +#include + +#include +#include +#include +#include +#include +#include + +#include "referee/app/ui/shape/cfs_scheduler.hpp" +#include "referee/app/ui/shape/shape.hpp" +#include "referee/command/interaction/header.hpp" + +namespace rmcs_core::referee::command::interaction { +using namespace app::ui; + +class Ui + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Ui() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + register_input("/referee/id", robot_id_); + register_input("/referee/game/stage", game_stage_); + register_input("/remote/keyboard", keyboard_); + + register_output("/referee/command/interaction/ui", ui_field_); + } + + void update() override { + if (*robot_id_ == rmcs_msgs::RobotId::UNKNOWN) { + *ui_field_ = Field{}; + return; + } + + if ((last_game_stage_ == rmcs_msgs::GameStage::UNKNOWN + && *game_stage_ != rmcs_msgs::GameStage::UNKNOWN) + || (last_game_stage_ != rmcs_msgs::GameStage::PREPARATION + && *game_stage_ == rmcs_msgs::GameStage::PREPARATION) + || (!last_keyboard_.r && keyboard_->r)) { + RemoteShape::force_revoke_all_id(); + resetting_ = 4; + } + last_game_stage_ = *game_stage_; + last_keyboard_ = *keyboard_; + + if (resetting_) { + *ui_field_ = Field{[this](std::byte* buffer) { + --resetting_; + return write_resetting_field(buffer); + }}; + return; + } + + if (CfsScheduler::empty()) { + *ui_field_ = Field{}; + return; + } + + *ui_field_ = Field{[this](std::byte* buffer) { return write_updating_field(buffer); }}; + } + +private: + size_t write_resetting_field(std::byte* buffer) const { + size_t written = 0; + + auto& header = *new (buffer + written) Header{}; + header.command_id = 0x0100; // Clear shapes + auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; + header.sender_id = full_robot_id; + header.receiver_id = full_robot_id.client(); + written += sizeof(Header); + + struct Command { + uint8_t type; + uint8_t layer; + }; + auto& command = *new (buffer + written) Command{}; + command.type = 2; // Clear all layers + command.layer = 0; + written += sizeof(Command); + + return written; + } + + size_t write_updating_field(std::byte* buffer) const { + size_t written = 0; + + auto& header = *new (buffer + written) Header{}; + auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; + header.sender_id = full_robot_id; + header.receiver_id = full_robot_id.client(); + written += sizeof(Header); + + int slot = 0; + intptr_t updated[7]; + for (auto it = CfsScheduler::get_update_iterator(); it && slot < 7;) { + // Ignore text shape unless it is the first. + if (it->is_text_shape()) { + if (slot == 0) { + header.command_id = 0x0110; // Draw text shape + return written + it.update().write(buffer + written); + } else { + it.ignore(); + continue; + } + } + + auto operation = it->predict_update(); + if (operation == Shape::Operation::NO_OPERATION) { + it.ignore(); + continue; + } + + // Shapes are always aligned, so the last bits can be used to store information. + auto identification = + reinterpret_cast(it.get()) | (operation == Shape::Operation::ADD); + // Ignore identical shapes that operate identically. + if (std::find(updated, updated + slot, identification) != updated + slot) { + it.ignore(); + continue; + } + + written += it.update().write(buffer + written); + + updated[slot++] = identification; + } + + constexpr std::pair optional_packet[4] = { + {1, 0x0101}, // Draw 1 shape + {2, 0x0102}, // Draw 2 shapes + {5, 0x0103}, // Draw 5 shapes + {7, 0x0104}, // Draw 7 shapes + }; + for (const auto& [shape_count, command_id] : optional_packet) { + if (slot <= shape_count) { + for (; slot < shape_count; ++slot) { + written += Shape::no_operation_description().write(buffer + written); + } + header.command_id = command_id; + break; + } + } + + return written; + } + + InputInterface robot_id_; + + InputInterface game_stage_; + rmcs_msgs::GameStage last_game_stage_ = rmcs_msgs::GameStage::UNKNOWN; + + InputInterface keyboard_; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + int resetting_ = 0; + + OutputInterface ui_field_; +}; + +} // namespace rmcs_core::referee::command::interaction + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::command::interaction::Ui, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/map_marker.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/map_marker.cpp new file mode 100644 index 00000000..e69de29b diff --git a/rmcs_ws/src/rmcs_core/src/referee/frame.hpp b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp new file mode 100644 index 00000000..d8a406e4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/frame.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include + +namespace rmcs_core::referee { + +constexpr uint8_t sof_value = 0xa5; +constexpr size_t frame_data_max_length = 1024; + +struct __attribute__((packed)) FrameHeader { + uint8_t sof; + uint16_t data_length; + uint8_t sequence; + uint8_t crc8; +}; + +struct __attribute__((packed)) FrameBody { + uint16_t command_id; + std::byte data[frame_data_max_length]; +}; + +struct __attribute__((packed)) Frame { + FrameHeader header; + FrameBody body; +}; + +} // namespace rmcs_core::referee \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp new file mode 100644 index 00000000..3f577d94 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -0,0 +1,205 @@ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "referee/frame.hpp" +#include "referee/status/field.hpp" + +namespace rmcs_core::referee { +using namespace status; + +class Status + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Status() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + register_input("/referee/serial", serial_); + + register_output("/referee/game/stage", game_stage_, rmcs_msgs::GameStage::UNKNOWN); + + register_output("/referee/id", robot_id_, rmcs_msgs::RobotId::UNKNOWN); + register_output("/referee/shooter/cooling", robot_shooter_cooling_, 0); + register_output("/referee/shooter/heat_limit", robot_shooter_heat_limit_, 0); + register_output("/referee/chassis/power_limit", robot_chassis_power_limit_, 0.0); + register_output("/referee/chassis/power", robot_chassis_power_, 0.0); + register_output("/referee/chassis/buffer_energy", robot_buffer_energy_, 60.0); + register_output("/referee/robots/hp", robots_hp_); + register_output("/referee/shooter/bullet_allowance", robot_bullet_allowance_, false); + + register_output("/referee/shooter/initial_speed", robot_initial_speed_, false); + register_output("/referee/shooter/shoot_timestamp", robot_shoot_timestamp_, false); + + robot_status_watchdog_.reset(5'000); + } + + void update() override { + if (!serial_.active()) + return; + + if (cache_size_ >= sizeof(frame_.header)) { + auto frame_size = sizeof(frame_.header) + sizeof(frame_.body.command_id) + + frame_.header.data_length + sizeof(uint16_t); + cache_size_ += serial_->read( + reinterpret_cast(&frame_) + cache_size_, frame_size - cache_size_); + + if (cache_size_ == frame_size) { + cache_size_ = 0; + if (rmcs_utility::dji_crc::verify_crc16(&frame_, frame_size)) { + process_frame(); + } else { + RCLCPP_WARN(logger_, "Body crc16 invalid"); + } + } + } else { + auto result = rmcs_utility::receive_package( + const_cast(*serial_), frame_.header, cache_size_, + static_cast(0xa5), [](const FrameHeader& header) { + return rmcs_utility::dji_crc::verify_crc8(header); + }); + if (result == rmcs_utility::ReceiveResult::HEADER_INVALID) { + RCLCPP_WARN(logger_, "Header start invalid"); + } else if (result == rmcs_utility::ReceiveResult::VERIFY_INVALID) { + RCLCPP_WARN(logger_, "Header crc8 invalid"); + } + } + + if (game_status_watchdog_.tick()) { + RCLCPP_INFO(logger_, "Game status receiving timeout. Set stage to unknown."); + *game_stage_ = rmcs_msgs::GameStage::UNKNOWN; + } + if (robot_status_watchdog_.tick()) { + RCLCPP_ERROR(logger_, "Robot status receiving timeout. Set to safe indicators."); + *robot_shooter_cooling_ = safe_shooter_cooling; + *robot_shooter_heat_limit_ = safe_shooter_heat_limit; + *robot_chassis_power_limit_ = safe_chassis_power_limit; + } + if (power_heat_data_watchdog_.tick()) { + RCLCPP_ERROR(logger_, "Power heat data receiving timeout. Set to initial values."); + *robot_chassis_power_ = 0.0; + *robot_buffer_energy_ = 60.0; + } + } + +private: + void process_frame() { + auto command_id = frame_.body.command_id; + if (command_id == 0x0001) + update_game_status(); + if (command_id == 0x0003) + update_game_robot_hp(); + else if (command_id == 0x0201) + update_robot_status(); + else if (command_id == 0x0202) + update_power_heat_data(); + else if (command_id == 0x0203) + update_robot_position(); + else if (command_id == 0x0206) + update_hurt_data(); + else if (command_id == 0x0207) + update_shoot_data(); + else if (command_id == 0x0208) + update_bullet_allowance(); + else if (command_id == 0x020B) + update_game_robot_position(); + } + + void update_game_status() { + auto& data = reinterpret_cast(frame_.body.data); + + *game_stage_ = static_cast(data.game_stage); + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) + game_status_watchdog_.reset(30'000); + else + game_status_watchdog_.reset(5'000); + } + + void update_game_robot_hp() {} + + void update_robot_status() { + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) + robot_status_watchdog_.reset(60'000); + else + robot_status_watchdog_.reset(5'000); + + auto& data = reinterpret_cast(frame_.body.data); + + *robot_id_ = static_cast(data.robot_id); + *robot_shooter_cooling_ = data.shooter_barrel_cooling_value; + *robot_shooter_heat_limit_ = static_cast(1000) * data.shooter_barrel_heat_limit; + *robot_chassis_power_limit_ = static_cast(data.chassis_power_limit); + } + + void update_power_heat_data() { + power_heat_data_watchdog_.reset(3'000); + + auto& data = reinterpret_cast(frame_.body.data); + *robot_chassis_power_ = data.chassis_power; + *robot_buffer_energy_ = static_cast(data.buffer_energy); + } + + void update_robot_position() {} + + void update_hurt_data() {} + + void update_shoot_data() { + auto& data = reinterpret_cast(frame_.body.data); + *robot_initial_speed_ = data.initial_speed; + + const auto now = std::chrono::high_resolution_clock::now(); + *robot_shoot_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); + } + + void update_bullet_allowance() { + auto& data = reinterpret_cast(frame_.body.data); + *robot_bullet_allowance_ = data.bullet_allowance_17mm; + } + + void update_game_robot_position() {} + + // When referee system loses connection unexpectedly, + // use these indicators make sure the robot safe. + // Muzzle: Cooling priority with level 1 + static constexpr int64_t safe_shooter_cooling = 40; + static constexpr int64_t safe_shooter_heat_limit = 50'000; + // Chassis: Health priority with level 1 + static constexpr double safe_chassis_power_limit = 45; + + rclcpp::Logger logger_; + + InputInterface serial_; + Frame frame_; + size_t cache_size_ = 0; + + rmcs_utility::TickTimer game_status_watchdog_; + OutputInterface game_stage_; + + rmcs_utility::TickTimer robot_status_watchdog_; + OutputInterface robot_id_; + OutputInterface robot_shooter_cooling_, robot_shooter_heat_limit_; + OutputInterface robot_chassis_power_limit_; + + rmcs_utility::TickTimer power_heat_data_watchdog_; + OutputInterface robot_chassis_power_; + OutputInterface robot_buffer_energy_; + + OutputInterface robots_hp_; + OutputInterface robot_bullet_allowance_; + + OutputInterface robot_initial_speed_; + OutputInterface robot_shoot_timestamp_; +}; + +} // namespace rmcs_core::referee + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Status, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp new file mode 100644 index 00000000..5cc1129f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include + +namespace rmcs_core::referee::status { + +struct __attribute__((packed)) GameStatus { + uint8_t game_type : 4; + uint8_t game_stage : 4; + uint16_t stage_remain_time; + uint64_t sync_timestamp; +}; + +struct __attribute__((packed)) GameRobotHp { + uint16_t red_1; + uint16_t red_2; + uint16_t red_3; + uint16_t red_4; + uint16_t red_5; + uint16_t red_7; + uint16_t red_outpost; + uint16_t red_base; + uint16_t blue_1; + uint16_t blue_2; + uint16_t blue_3; + uint16_t blue_4; + uint16_t blue_5; + uint16_t blue_7; + uint16_t blue_outpost; + uint16_t blue_base; +}; + +struct __attribute__((packed)) RobotStatus { + uint8_t robot_id; + uint8_t robot_level; + uint16_t current_hp; + uint16_t maximum_hp; + uint16_t shooter_barrel_cooling_value; + uint16_t shooter_barrel_heat_limit; + uint16_t chassis_power_limit; + uint8_t power_management_gimbal_output : 1; + uint8_t power_management_chassis_output : 1; + uint8_t power_management_shooter_output : 1; +}; + +struct __attribute__((packed)) PowerHeatData { + uint16_t chassis_voltage; + uint16_t chassis_current; + float chassis_power; + uint16_t buffer_energy; + uint16_t shooter_17mm_1_barrel_heat; + uint16_t shooter_17mm_2_barrel_heat; + uint16_t shooter_42mm_barrel_heat; +}; + +struct __attribute__((packed)) RobotPosition { + float x; + float y; + float angle; +}; + +struct __attribute__((packed)) HurtData { + uint8_t armor_id : 4; + uint8_t reason : 4; +}; + +struct __attribute__((packed)) ShootData { + uint8_t bullet_type; + uint8_t shooter_number; + uint8_t launching_frequency; + float initial_speed; +}; + +struct __attribute__((packed)) BulletAllowance { + uint16_t bullet_allowance_17mm; + uint16_t bullet_allowance_42mm; + uint16_t remaining_gold_coin; +}; + +struct __attribute__((packed)) GameRobotPosition { + float hero_x; + float hero_y; + float engineer_x; + float engineer_y; + float infantry_3_x; + float infantry_3_y; + float infantry_4_x; + float infantry_4_y; + float infantry_5_x; + float infantry_5_y; +}; + +} // namespace rmcs_core::referee::status \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/referee/vision.cpp b/rmcs_ws/src/rmcs_core/src/referee/vision.cpp new file mode 100644 index 00000000..29709257 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/referee/vision.cpp @@ -0,0 +1,157 @@ +#include +#include "rmcs_msgs/keyboard.hpp" +#include "rmcs_msgs/mouse.hpp" +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "referee/frame.hpp" + +#include + +namespace rmcs_core::referee { + +class Vision + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Vision() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) { + + std::string path; + if (get_parameter("path", path)) + RCLCPP_INFO(get_logger(), "Path: %s", path.c_str()); + else + throw std::runtime_error{"Unable to get parameter 'path'"}; + + const std::string stty_command = "stty -F " + path + " raw"; + if (std::system(stty_command.c_str()) != 0) + throw std::runtime_error{"Unable to call '" + stty_command + "'"}; + // register_input("/referee/vision/serial", serial_); + register_output("/referee/vision/serial", serial_, path, 921600, serial::Timeout::simpleTimeout(0)); + + register_output("/referee/vision/custom", custom_data_ ); + register_output("/referee/vision/controller_keyboard", controller_keyboard_, rmcs_msgs::Keyboard::zero()); + register_output("/referee/vision/controller_mouse", controller_mouse_, rmcs_msgs::Mouse::zero()); + + std::fill((*custom_data_).begin(), (*custom_data_).end(), 0); + + custom_watchdog_.reset(5'000); + controller_watchdog_.reset(5'000); + } + + void update() override { + + if (!serial_.active() || !serial_->available()) + return; + + if (cache_size_ >= sizeof(frame_.header)) { + + auto frame_size = sizeof(frame_.header) + sizeof(frame_.body.command_id) + + frame_.header.data_length + sizeof(uint16_t); + cache_size_ += serial_->read( + reinterpret_cast(&frame_) + cache_size_, frame_size - cache_size_); + + if (cache_size_ == frame_size) { + cache_size_ = 0; + if (rmcs_utility::dji_crc::verify_crc16(&frame_, frame_size)) { + process_frame(); + } else { + RCLCPP_WARN(logger_, "Body crc16 invalid"); + } + } + } else { + auto result = rmcs_utility::receive_package( + *serial_, frame_.header, cache_size_, + static_cast(0xa5), [](const FrameHeader& header) { + return rmcs_utility::dji_crc::verify_crc8(header); + }); + if (result == rmcs_utility::ReceiveResult::HEADER_INVALID) { + // RCLCPP_WARN(logger_, "Header start invalid"); + } else if (result == rmcs_utility::ReceiveResult::VERIFY_INVALID) { + // RCLCPP_WARN(logger_, "Header crc8 invalid"); + } else if (result == rmcs_utility::ReceiveResult::TIMEOUT) { + // RCLCPP_WARN(logger_, "Referee vision package receive timeout."); + } else { + // RCLCPP_WARN(logger_, "Referee vision package receive success."); + } + } + + if (custom_watchdog_.tick()) { + RCLCPP_ERROR(logger_, "Vision custom data receiving timeout. Set data to zero."); + std::fill((*custom_data_).begin(), (*custom_data_).end(), 0); + } + if (controller_watchdog_.tick()) { + RCLCPP_ERROR(logger_, "Vision controller data receiving timeout. Set data to zero."); + *controller_keyboard_ = rmcs_msgs::Keyboard::zero(); + *controller_mouse_ = rmcs_msgs::Mouse::zero(); + } + } + +private: + void process_frame() { + auto command_id = frame_.body.command_id; + + if (command_id == 0x0302) + // RCLCPP_INFO(logger_, "Receiving cmannd_id %04x.", command_id); + update_custom_data(); + if (command_id == 0x0304) + update_controller_data(); + } + + void update_custom_data() { + std::copy( + reinterpret_cast(frame_.body.data), + reinterpret_cast(frame_.body.data) + frame_.header.data_length, + (*custom_data_).begin() + ); + // RCLCPP_INFO(this->get_logger(),"%d",frame_.header.data_length); + // auto custom_data = *custom_data_; + // std::ostringstream oss; + // oss << std::hex << std::setfill('0'); // 设置十六进制和补零格式 + // for (size_t i = 0; i < custom_data.size(); ++i) { + // // 将每个字节转为两位十六进制，并添加空格（最后一个不加） + // oss << std::setw(2) << static_cast(custom_data[i]); + // if (i != custom_data.size() - 1) { + // oss << " "; + // } + // } + // std::string hex_str = oss.str(); + // RCLCPP_INFO(get_logger(), "%s", hex_str.c_str()); + custom_watchdog_.reset(500); + } + + void update_controller_data() { + controller_watchdog_.reset(500); + + // TODO + // auto data = frame_.body.data; + } + rclcpp::Logger logger_; + + OutputInterface serial_; + // InputInterface serial_; + Frame frame_; + size_t cache_size_ = 0; + + rmcs_utility::TickTimer custom_watchdog_; + OutputInterface> custom_data_; + + rmcs_utility::TickTimer controller_watchdog_; + OutputInterface controller_keyboard_; + OutputInterface controller_mouse_; +}; + +} // namespace rmcs_core::referee + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Vision, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp index 5b7239ac..cba9b55f 100644 --- a/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp +++ b/rmcs_ws/src/rmcs_executor/include/rmcs_executor/component.hpp @@ -31,7 +31,7 @@ class Component { virtual void update() = 0; template - class InputInterface { + requires(!std::is_reference_v && !std::is_unbounded_array_v) class InputInterface { public: friend class Component; @@ -42,16 +42,36 @@ class Component { InputInterface(InputInterface&&) = delete; InputInterface& operator=(InputInterface&&) = delete; - ~InputInterface() = default; + ~InputInterface() { + if (delete_data_when_deconstruct) { + if constexpr (std::is_array_v) { + delete[] data_pointer_; + } else { + delete data_pointer_; + } + } + } [[nodiscard]] bool active() const { return activated; } [[nodiscard]] bool ready() const { return data_pointer_ != nullptr; } - void bind_directly(T& destination) { + template + void make_and_bind_directly(Args&&... args) { if (ready()) throw std::runtime_error("The interface has already been bound to somewhere"); + + data_pointer_ = new T(std::forward(args)...); + activated = true; + + delete_data_when_deconstruct = true; + } + + void bind_directly(const T& destination) { + if (ready()) + throw std::runtime_error("The interface has already been bound to somewhere"); + + data_pointer_ = const_cast(&destination); activated = true; - data_pointer_ = &destination; } const T* operator->() const { return data_pointer_; } @@ -65,10 +85,12 @@ class Component { T* data_pointer_ = nullptr; bool activated = false; + + bool delete_data_when_deconstruct = false; }; template - class OutputInterface { + requires(!std::is_reference_v && !std::is_unbounded_array_v) class OutputInterface { public: friend class Component; diff --git a/rmcs_ws/src/rmcs_executor/src/executor.hpp b/rmcs_ws/src/rmcs_executor/src/executor.hpp index a818d50a..3924c829 100644 --- a/rmcs_ws/src/rmcs_executor/src/executor.hpp +++ b/rmcs_ws/src/rmcs_executor/src/executor.hpp @@ -76,6 +76,11 @@ class Executor final : public rclcpp::Node { component->dependency_count_ = 0; component->wanted_by_.clear(); for (auto& output : component->output_list_) { + // RCLCPP_INFO( + // this->get_logger(), + // "%s: %s\n", + // component->get_component_name().c_str(), + // output.name.c_str()); if (!output_map.emplace(output.name, &output).second) throw std::runtime_error{"Duplicate names of output"}; user_output_map.emplace(output.name, output.type); diff --git a/rmcs_ws/src/rmcs_msgs/CMakeLists.txt b/rmcs_ws/src/rmcs_msgs/CMakeLists.txt index eef4b7da..550f5d67 100644 --- a/rmcs_ws/src/rmcs_msgs/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_msgs/CMakeLists.txt @@ -19,11 +19,6 @@ foreach(MSG ${RMCS_MSGS_MSGS}) set(MSG_PATH "${MSG_PATH};msg/${MSG_NAME}.msg") endforeach() -rosidl_generate_interfaces(${PROJECT_NAME} - ${MSG_PATH} - DEPENDENCIES builtin_interfaces geometry_msgs -) - include_directories(${PROJECT_SOURCE_DIR}/include) ament_auto_package() \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/arm_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/arm_mode.hpp new file mode 100644 index 00000000..4bc2b609 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/arm_mode.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class ArmMode : uint8_t { + Auto_Gold_Left, + Auto_Gold_Right, + Auto_Gold_Mid, + Auto_Sliver, + Auto_Walk, + Auto_Spin, + Auto_Ground, + Auto_Storage_LB, + Auto_Storage_RB, + Auto_Extract, + Auto_Up_Stairs, + Auto_Down_Stairs, + Customer, + DT7_Control_Position, + DT7_Control_Orientation, + None +}; + +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp index e534ce0b..ee4ceb3c 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/chassis_mode.hpp @@ -5,10 +5,11 @@ namespace rmcs_msgs { enum class ChassisMode : uint8_t { - AUTO = 0, - SPIN = 1, - STEP_DOWN = 2, - LAUNCH_RAMP = 3, + SPIN = 1, + Flow, + Yaw_Free, + None, + Up_Stairs }; } // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/leg_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/leg_mode.hpp new file mode 100644 index 00000000..cb9cb211 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/leg_mode.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class LegMode : uint8_t { + Four_Wheel, + Six_Wheel, + None, + Up_Stairs, + Down_Stairs +}; + +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/up_stairs_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/up_stairs_mode.hpp new file mode 100644 index 00000000..2a34881d --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/up_stairs_mode.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +enum class UpStairsMode : uint8_t { + Step_By_One, + Step_By_Two, + Auto +}; + +} // namespace rmcs_msgs \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_msgs/package.xml b/rmcs_ws/src/rmcs_msgs/package.xml index 2243aa6d..f8177d0e 100644 --- a/rmcs_ws/src/rmcs_msgs/package.xml +++ b/rmcs_ws/src/rmcs_msgs/package.xml @@ -15,7 +15,7 @@ std_msgs geometry_msgs builtin_interfaces - + rosidl_default_generators ament_cmake diff --git a/rmcs_ws/src/rmcs_referee b/rmcs_ws/src/rmcs_referee deleted file mode 160000 index d0b3792f..00000000 --- a/rmcs_ws/src/rmcs_referee +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d0b3792f4ae0e97beb786433fcace041301602f3 diff --git a/rmcs_ws/src/rmcs_utility/CMakeLists.txt b/rmcs_ws/src/rmcs_utility/CMakeLists.txt new file mode 100644 index 00000000..e9c09fbe --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.12) +project(rmcs_utility) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c11") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O2 -Wall -Wextra -Wpedantic) +endif() + +find_package (ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies () + +include_directories(${PROJECT_SOURCE_DIR}/include) + +ament_auto_package() \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/LICENSE b/rmcs_ws/src/rmcs_utility/LICENSE new file mode 100644 index 00000000..f288702d --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/LICENSE @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp new file mode 100644 index 00000000..620c6023 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/crc/dji_crc.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include +#include + +namespace rmcs_utility::dji_crc { +namespace internal { + +template +inline auto& get_tail(T& data) { + return *reinterpret_cast(reinterpret_cast(&data) + sizeof(T) - sizeof(uint8_t)); +} + +constexpr uint8_t crc8_init = 0xff; +constexpr uint8_t crc8_table[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, + 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, + 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, + 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, + 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, + 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a, + 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, + 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, + 0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, + 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, + 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, + 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73, + 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, + 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, + 0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, + 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, +}; + +constexpr uint16_t crc16_init = 0xffff; +constexpr uint16_t crc16_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, + 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, + 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, + 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, + 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, + 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, + 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, 0x3bd3, 0x2a5a, + 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, + 0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, + 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e, 0xf687, 0xc41c, 0xd595, + 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78}; + +} // namespace internal + +inline uint8_t calculate_crc8(const void* data, size_t length) { + auto* p = reinterpret_cast(data); + auto result = internal::crc8_init; + while (length--) + result = internal::crc8_table[result ^ (*p++)]; + return result; +} + +inline bool verify_crc8(const void* data, size_t length) { + auto checksum = calculate_crc8(data, length - sizeof(uint8_t)); + return checksum == *(reinterpret_cast(data) + length - sizeof(uint8_t)); +} +template +inline bool verify_crc8(const T& package) { + static_assert(sizeof(T) > sizeof(uint8_t)); + return verify_crc8(&package, sizeof(package)); +} + +inline void append_crc8(void* data, size_t length) { + auto checksum = calculate_crc8(data, length - sizeof(uint8_t)); + *(reinterpret_cast(data) + length - sizeof(uint8_t)) = checksum; +} +template +inline void append_crc8(T& package) { + static_assert(sizeof(T) > sizeof(uint8_t)); + append_crc8(&package, sizeof(package)); +} + +inline uint16_t calculate_crc16(const void* data, size_t length) { + auto* p = reinterpret_cast(data); + uint16_t result = internal::crc16_init; + while (length--) { + result = (result >> 8) ^ internal::crc16_table[(result ^ (*p++)) & 0x00ff]; + } + return result; +} + +inline bool verify_crc16(const void* data, size_t length) { + auto checksum = calculate_crc16(data, length - sizeof(uint16_t)); + return checksum + == *reinterpret_cast( + reinterpret_cast(data) + length - sizeof(uint16_t)); +} +template +inline bool verify_crc16(const T& package) { + static_assert(sizeof(T) > sizeof(uint16_t)); + return verify_crc16(&package, sizeof(package)); +} + +inline void append_crc16(void* data, size_t length) { + auto checksum = calculate_crc16(data, length - sizeof(uint16_t)); + *reinterpret_cast(reinterpret_cast(data) + length - sizeof(uint16_t)) = + checksum; +} +template +inline void append_crc16(T& package) { + static_assert(sizeof(T) > sizeof(uint16_t)); + append_crc16(&package, sizeof(package)); +} + +} // namespace rmcs_utility::dji_crc \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp new file mode 100644 index 00000000..4b5fe9c4 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/double_buffer.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include +#include + +#include +#include +#include + +namespace rmcs_utility { + +/// @brief Implements a reliable, lock-free, arbitrary-length cross-thread buffer. +/// @tparam T The type of data to be stored in the buffer. +/// @tparam i_am_very_sure_this_is_trivially_copyable_and_destructible A flag to bypass type checks. +/// Set to `true` only if you are certain that `T` is trivially copyable and destructible. +template +requires(std::is_trivially_copyable_v && std::is_trivially_destructible_v) + || i_am_very_sure_this_is_trivially_copyable_and_destructible class DoubleBuffer { +public: + /// @brief Writes data to the buffer in a thread-safe manner. + /// @param data The data to be written to the buffer. + void write(const T& data) noexcept { + const uint64_t current = current_.load(std::memory_order_relaxed); + const uint64_t next = current + 1; + std::memcpy(&buffers_[next & 1], &data, sizeof(T)); + current_.store(next, std::memory_order_release); + } + + /// @brief Reads data from the buffer in a thread-safe manner. + /// @param data The variable where the read data will be stored. + /// @return Returns `true` if the read was successful, `false` otherwise. + /// @note Under high write pressure, consumers may occasionally read incorrect data. + /// The function explicitly returns `false` to signal failure. Readers can choose to retry + /// (e.g., using a `while` loop) or cancel the operation. + /// @note If the function returns `false`, the variable `data` will be filled with invalid data. + bool read(T& data) noexcept { + const uint64_t current = current_.load(std::memory_order_acquire); + std::memcpy(&data, &buffers_[current & 1], sizeof(T)); + return current_.load(std::memory_order_acquire) == current; + } + +private: + /// @brief Internal buffer structure to hold the data. + /// @note The buffer alignment is set to the maximum of 64 bytes and `T`'s natural alignment. + /// This serves two purposes: + /// 1. Prevents false sharing by ensuring 64-byte cache-line separation. + /// 2. Guarantees proper alignment for `T` type operations. + struct alignas(std::max(size_t{64}, alignof(T))) Buffer { + std::byte data[sizeof(T)]; + }; + + alignas(64) std::atomic current_{0}; + alignas(64) Buffer buffers_[2]; +}; + +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/eigen_structured_bindings.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/eigen_structured_bindings.hpp new file mode 100644 index 00000000..2bab73d5 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/eigen_structured_bindings.hpp @@ -0,0 +1,45 @@ +#pragma once + +#include + +// https://gist.github.com/mtao/58e6859ac4ac2300ddb9e1050b9ed701 + +// IWYU pragma: always_keep + +namespace Eigen { + +template +inline auto& get(Eigen::DenseCoeffsBase& p) { + return p.coeffRef(D); +} + +template +inline auto get(const Eigen::DenseCoeffsBase& p) { + return p.coeff(D); +} + +template +concept StaticSizePlainObjectBase = + std::derived_from> + && (Derived::RowsAtCompileTime > 0 && Derived::ColsAtCompileTime > 0); + +template +concept StaticSizeEigenBase = std::derived_from> + && (Derived::RowsAtCompileTime > 0 && Derived::ColsAtCompileTime > 0); + +} // namespace Eigen + +namespace std { + +template +requires Eigen::StaticSizeEigenBase struct tuple_size { + constexpr static int value = Derived::RowsAtCompileTime * Derived::ColsAtCompileTime; +}; +template +requires Eigen::StaticSizeEigenBase + && (D < Derived::RowsAtCompileTime * Derived::ColsAtCompileTime) +struct tuple_element { + using type = typename Derived::Scalar; +}; + +} // namespace std \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp new file mode 100644 index 00000000..2c5ff84f --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/fps_counter.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include + +namespace rmcs_utility { + +class FpsCounter { +public: + explicit FpsCounter( + std::chrono::steady_clock::duration measurement_window = std::chrono::seconds(1)) + : measurement_window_(measurement_window) + , start_(std::chrono::steady_clock::now()) {} + + bool count() { + ++count_; + + auto now = std::chrono::steady_clock::now(); + auto elapsed_time = now - start_; + if (elapsed_time >= measurement_window_) { + start_ = now; + fps_ = double(count_) / std::chrono::duration(elapsed_time).count(); + count_ = 0; + return true; + } + + return false; + } + + double fps() const { return fps_; } + +private: + std::chrono::steady_clock::duration measurement_window_; + std::chrono::steady_clock::time_point start_; + + int64_t count_ = 0; + double fps_ = 0; +}; + +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp new file mode 100644 index 00000000..60571f8e --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/package_receive.hpp @@ -0,0 +1,91 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace rmcs_utility { + +enum class ReceiveResult : uint8_t { + SUCCESS = 0, + TIMEOUT = 1, + HEADER_INVALID = 2, + VERIFY_INVALID = 3 +}; + +template +concept is_byte = + std::is_same_v || std::is_same_v || std::is_same_v; + +template +concept is_readable_stream = requires(SerialT& serial, ByteT* pointer, size_t size) { + { serial.read(pointer, size) } -> std::convertible_to; +}; + +template +concept is_verify_function = requires(const F& f, const PackageT& package) { + { f(package) } -> std::convertible_to; +}; + +template +requires std::is_trivially_copyable_v inline auto receive_package( + is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, + const is_verify_function auto& header_verify, + const is_verify_function auto& verify) -> ReceiveResult { + if (cache_size == sizeof(PackageT)) + return ReceiveResult::SUCCESS; + + auto* buffer_pointer = reinterpret_cast(&buffer); + cache_size += stream.read(buffer_pointer + cache_size, sizeof(PackageT) - cache_size); + + if (cache_size == 0 || cache_size < header_size) + return ReceiveResult::TIMEOUT; + + ReceiveResult result; + if (header_size == 0 || header_verify(buffer)) { + if (cache_size != sizeof(PackageT)) + return ReceiveResult::TIMEOUT; + if (verify(buffer)) + return ReceiveResult::SUCCESS; + else + result = ReceiveResult::VERIFY_INVALID; + } else { + result = ReceiveResult::HEADER_INVALID; + } + + while (true) { + memmove(buffer_pointer, buffer_pointer + 1, --cache_size); + if (cache_size == 0 || cache_size < header_size || header_size == 0 + || header_verify(*std::launder(reinterpret_cast(buffer_pointer)))) { + break; + } + } + return result; +} + +template +requires std::is_trivially_copyable_v inline auto receive_package( + is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, HeaderT header, + const is_verify_function auto& verify) -> ReceiveResult { + return receive_package( + stream, buffer, cache_size, + [header](const PackageT& package) { + HeaderT actual_header; + std::memcpy(&actual_header, &package, sizeof(HeaderT)); + return actual_header == header; + }, + verify); +} + +template +requires std::is_trivially_copyable_v inline auto receive_package( + is_readable_stream auto& stream, PackageT& buffer, size_t& cache_size, + const is_verify_function auto& verify) -> ReceiveResult { + return receive_package<0, std::byte>( + stream, buffer, cache_size, [](const PackageT&) { return true; }, verify); +} + +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp new file mode 100644 index 00000000..1c62c591 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/ring_buffer.hpp @@ -0,0 +1,190 @@ +#pragma once + +#include +#include + +#include +#include + +namespace rmcs_utility { + +template +class RingBuffer { +public: + constexpr explicit RingBuffer(size_t size) { + if (size <= 2) + size = 2; + else + size = round_up_to_next_power_of_2(size); + mask = size - 1; + storage_ = new Storage[size]; + }; + + ~RingBuffer() { + clear(); + delete[] storage_; + } + + size_t max_size() const { return mask + 1; } + + /*! + * \brief Check how many elements can be read from the buffer + * \return Number of elements that can be read + */ + size_t readable() const { + return in_.load(std::memory_order::acquire) - out_.load(std::memory_order::relaxed); + } + + /*! + * \brief Check how many elements can be written into the buffer + * \return Number of free slots that can be be written + */ + size_t writeable() const { + return max_size() + - (in_.load(std::memory_order::relaxed) - out_.load(std::memory_order::acquire)); + } + + /*! + * \brief Gets the first element in the buffer on consumed side + * + * It is safe to use and modify item contents only on consumer side + * + * \return Pointer to first element, nullptr if buffer was empty + */ + T* front() { + auto out = out_.load(std::memory_order::relaxed); + + if (out == in_.load(std::memory_order::acquire)) + return nullptr; + else + return std::launder(reinterpret_cast(storage_[out & mask].data)); + } + + /*! + * \brief Gets the last element in the buffer on consumed side + * + * It is safe to use and modify item contents only on consumer side + * + * \return Pointer to last element, nullptr if buffer was empty + */ + T* back() { + auto in = in_.load(std::memory_order::acquire); + + if (in == out_.load(std::memory_order::relaxed)) + return nullptr; + else + return std::launder(reinterpret_cast(storage_[in & mask].data)); + } + + template + requires requires(F f, std::byte* storage) { f(storage); } + size_t emplace_back_multi(F construct_functor, size_t count = -1) { + auto in = in_.load(std::memory_order::relaxed); + auto out = out_.load(std::memory_order::acquire); + + auto writeable = max_size() - (in - out); + + if (count > writeable) + count = writeable; + if (!count) + return 0; + + auto offset = in & mask; + auto slice = std::min(count, max_size() - offset); + + for (size_t i = 0; i < slice; i++) + construct_functor(storage_[offset + i].data); + for (size_t i = 0; i < count - slice; i++) + construct_functor(storage_[i].data); + + std::atomic_signal_fence(std::memory_order::release); + in_.store(in + count, std::memory_order::release); + + return count; + } + + template + bool emplace_back(Args&&... args) { + return emplace_back_multi( + [&](std::byte* storage) { new (storage) T{std::forward(args...)}; }, 1); + } + + template + requires requires(F f) { T{f()}; } size_t push_back_multi(F generator, size_t count = -1) { + return emplace_back_multi([&](std::byte* storage) { new (storage) T{generator()}; }, count); + } + + bool push_back(const T& value) { + return emplace_back_multi([&](std::byte* storage) { new (storage) T{value}; }, 1); + } + bool push_back(T&& value) { + return emplace_back_multi( + [&](std::byte* storage) { new (storage) T{std::move(value)}; }, 1); + } + + template + requires requires(F f, T t) { f(std::move(t)); } + size_t pop_front_multi(F callback_functor, size_t count = -1) { + auto in = in_.load(std::memory_order::acquire); + auto out = out_.load(std::memory_order::relaxed); + + auto readable = in - out; + if (count > readable) + count = readable; + if (!count) + return 0; + + auto offset = out & mask; + auto slice = std::min(count, max_size() - offset); + + auto process = [&callback_functor](std::byte* storage) { + auto& element = *std::launder(reinterpret_cast(storage)); + callback_functor(std::move(element)); + std::destroy_at(&element); + }; + for (size_t i = 0; i < slice; i++) + process(storage_[offset + i].data); + for (size_t i = 0; i < count - slice; i++) + process(storage_[i].data); + + std::atomic_signal_fence(std::memory_order::release); + out_.store(out + count, std::memory_order::release); + + return count; + } + + template + requires requires(F f, T t) { f(std::move(t)); } bool pop_front(F&& callback_functor) { + return pop_front_multi(std::forward(callback_functor), 1); + } + + /*! + * \brief Clear buffer + * \return Number of elements that be erased + */ + size_t clear() { + return pop_front_multi([](T&&) {}); + } + +private: + constexpr static size_t round_up_to_next_power_of_2(size_t n) { + n--; + n |= n >> 1; + n |= n >> 2; + n |= n >> 4; + n |= n >> 8; + n |= n >> 16; + n |= n >> 32; + n++; + return n; + } + + size_t mask; + + std::atomic in_{0}, out_{0}; + struct Storage { + alignas(T) std::byte data[sizeof(T)]; + }* storage_; +}; + +}; // namespace rmcs_utility diff --git a/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tick_timer.hpp b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tick_timer.hpp new file mode 100644 index 00000000..0b82a238 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/include/rmcs_utility/tick_timer.hpp @@ -0,0 +1,23 @@ +#pragma once + +namespace rmcs_utility { + +class TickTimer { +public: + void reset(unsigned int cooldown) { counter_ = 2 * cooldown; } + + bool tick() { + if (counter_ == 0) [[unlikely]] { + counter_ = 1; + return true; + } else { + counter_ -= 2; + return false; + } + } + +private: + unsigned int counter_ = 1; +}; + +} // namespace rmcs_utility \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_utility/package.xml b/rmcs_ws/src/rmcs_utility/package.xml new file mode 100644 index 00000000..0ed7d1c9 --- /dev/null +++ b/rmcs_ws/src/rmcs_utility/package.xml @@ -0,0 +1,18 @@ + + + + rmcs_utility + 1.0.0 + Utility tool for serial port reception of data packets + Qzh + GPL-3.0-only + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/script/build.sh b/script/build.sh deleted file mode 100755 index 1d10b332..00000000 --- a/script/build.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/zsh - -workspace=/workspaces/rmcs/rmcs_ws - -cd ${workspace} && colcon build --merge-install --event-handlers console_direct+ - -source /workspaces/rmcs/rmcs_ws/install/setup.zsh -eval "$(register-python-argcomplete3 ros2)" -eval "$(register-python-argcomplete3 colcon)" \ No newline at end of file diff --git a/script/setup.sh b/script/setup.sh deleted file mode 100755 index 9e0b8ecb..00000000 --- a/script/setup.sh +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/bash - -function extensionInstallation { - echo -e "RMCS: start vscode extension installation." - code --install-extension streetsidesoftware.code-spell-checker - code --install-extension llvm-vs-code-extensions.vscode-clangd - code --install-extension twxs.cmake - echo -e "RMCS: finish vscode extension installation." -} - -# @todo(creeper) collect words -function codeSpellCheckSetup { - echo "code spell check setup." -} - -function clangdInstallation { - echo -e "RMCS: start clangd installation." - - if [[ -f /etc/apt/sources.list.d/llvm-apt.list ]]; then - echo -e "RMCS: \033[32m\033[0m exits, if you want to update this file," \ - "try \033[31m\"sudo rm /etc/apt/sources.list.d/llvm-apt.list\"\033[0m" - else - sudo apt-get install -y gnupg wget - wget -O /tmp/llvm-snapshot.gpg.key https://apt.llvm.org/llvm-snapshot.gpg.key - sudo apt-key add /tmp/llvm-snapshot.gpg.key - rm /tmp/llvm-snapshot.gpg.key - - sudo touch /etc/apt/sources.list.d/llvm-apt.list - sudo echo "deb https://mirrors.tuna.tsinghua.edu.cn/llvm-apt/jammy/ llvm-toolchain-jammy main" |\ - sudo tee -a /etc/apt/sources.list.d/llvm-apt.list - sudo apt-get update -y - fi - - version=`apt-cache search clangd- | grep clangd- | awk -F' ' '{print $1}' | sort -V | tail -1 | cut -d- -f2` - if command -v clangd-$version > /dev/null 2>&1; then - echo -e "RMCS:\033[32m clangd-$version\033[0m and\033[32m clang-format-$version\033[0m prepared." - else - sudo apt-get install -y clangd-$version clang-format-$version - fi - - if [[ -f /usr/bin/clangd ]]; then - echo -e "RMCS: \033[32m/usr/bin/clangd\033[0m exits, give up linking the clangd-20 to clangd." - else - sudo ln -s /usr/bin/clangd-$version /usr/bin/clangd - sudo ln -s /usr/bin/clang-format-$version /usr/bin/clang-format - fi - - echo -e "RMCS: finish clangd installation." -} - -function rmcsDependencyInstallation { - echo -e "RMCS: start rmcs installation." - cd /workspaces/rmcs/rmcs_ws/ - sudo rosdep install --from-paths src --ignore-src -r -y - echo -e "RMCS: finish rmcs installation." -} - -# sudo rm -rf /usr/local/lib/liblivox_lidar_sdk_* -# sudo rm -rf /usr/local/include/livox_lidar_* -function slamDependencyInstallation { - echo -e "RMCS: start slam dependency installation." - sudo apt install ros-humble-pcl-ros -y - git clone https://github.com/Livox-SDK/Livox-SDK2.git /tmp/livox-sdk - mkdir /tmp/livox-sdk/build && cd /tmp/livox-sdk/build - cmake .. && make -j 12 - sudo make install - sudo rm -r /tmp/livox-sdk - echo -e "if you want to install driver, clone this: "\ - "\033[32mhttps://github.com/Alliance-Algorithm/livox_ros_driver2.git\033[0m" - echo -e "RMCS: finish slam dependency installation." -} - -function show_help { - echo "Usage: $0 [--help] [--mode ]" - echo " --help Show this help message." - echo " --mode Set the mode to 'sentry' or 'infantry'." -} - -mode="" - -while [[ "$#" -gt 0 ]]; do - case $1 in - --help) - show_help - exit 0 - ;; - --mode) - shift - mode=$1 - if [[ "$mode" != "sentry" && "$mode" != "infantry" ]]; then - echo "Error: Mode must be 'sentry' or 'infantry'." - exit 1 - fi - ;; - *) - echo "Unknown option: $1" - exit 1 - ;; - esac - shift -done - -if [[ -n "$mode" ]]; then - echo "Mode set to: $mode" -else - echo "No mode set." -fi - -sudo apt-get update -y -clangdInstallation -extensionInstallation -rmcsDependencyInstallation - -if [[ "$mode" == "sentry" ]]; then - slamDependencyInstallation -fi \ No newline at end of file