diff --git a/.clang-format b/.clang-format index 99447491..646fc23e 100644 --- a/.clang-format +++ b/.clang-format @@ -13,7 +13,7 @@ MaxEmptyLinesToKeep: 1 AlignAfterOpenBracket: AlwaysBreak -AlignArrayOfStructures: Right +# AlignArrayOfStructures: Right AlignConsecutiveAssignments: Enabled: false diff --git a/.gitmodules b/.gitmodules index d6298295..4fe2e14d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +1,6 @@ [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/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git diff --git a/.script/complete/_record-lidar b/.script/complete/_record-lidar new file mode 100644 index 00000000..8a80093b --- /dev/null +++ b/.script/complete/_record-lidar @@ -0,0 +1,56 @@ +#compdef record-lidar + +_record_lidar_remote_sessions() { + local -a sessions + local line + local output + + output="$(ssh remote "screen -ls 2>/dev/null || true" 2>/dev/null)" + + sessions=() + while IFS= read -r line; do + if [[ "${line}" =~ [0-9]+\.(record-lidar-[^[:space:]]+) ]]; then + sessions+=("${match[1]}") + fi + done <<< "${output}" + + if (( ${#sessions[@]} > 0 )); then + compadd -- "${sessions[@]}" + fi +} + +if (( CURRENT == 2 )); then + compadd -- start list-session list-recorded stop-session + return +fi + +case "${words[2]}" in +start) + if [[ "${words[CURRENT]}" == -* ]] || [[ "${words[CURRENT-1]}" == "start" ]]; then + compadd -- --seconds --help -h + fi + ;; +list-session) + if [[ "${words[CURRENT]}" == -* ]] || [[ "${words[CURRENT-1]}" == "list-session" ]]; then + compadd -- --help -h + fi + ;; +list-recorded) + if [[ "${words[CURRENT]}" == -* ]] || [[ "${words[CURRENT-1]}" == "list-recorded" ]]; then + compadd -- --help -h + fi + ;; +stop-session) + if [[ "${words[CURRENT-1]}" == "--session" ]]; then + _record_lidar_remote_sessions + return + fi + + if [[ "${words[CURRENT]}" == -* ]] || [[ "${words[CURRENT-1]}" == "stop-session" ]]; then + compadd -- --session --help -h + fi + ;; +*) + _message 'unknown subcommand' + ;; +esac diff --git a/.script/complete/_save-map-once b/.script/complete/_save-map-once new file mode 100644 index 00000000..580dd804 --- /dev/null +++ b/.script/complete/_save-map-once @@ -0,0 +1,4 @@ +#compdef save-map-once + +_arguments \ + '1:mode:(remote local)' diff --git a/.script/foxglove b/.script/foxglove deleted file mode 100755 index 4181a4a3..00000000 --- a/.script/foxglove +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -source ~/env_setup.bash - -ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765 diff --git a/.script/launch-foxglove b/.script/launch-foxglove new file mode 100755 index 00000000..f7a9749e --- /dev/null +++ b/.script/launch-foxglove @@ -0,0 +1,3 @@ +#! /usr/bin/env bash + +ssh-remote "bash -lc 'source ~/env_setup.bash && ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=8765'" diff --git a/.script/launch-navigation b/.script/launch-navigation new file mode 100755 index 00000000..986914be --- /dev/null +++ b/.script/launch-navigation @@ -0,0 +1,3 @@ +#! /usr/bin/env bash + +ssh-remote "bash -lc 'source ~/env_setup.bash && ros2 launch rmcs-navigation custom.launch.py mode:=online'" diff --git a/.script/record-lidar b/.script/record-lidar new file mode 100755 index 00000000..e102cbde --- /dev/null +++ b/.script/record-lidar @@ -0,0 +1,400 @@ +#!/bin/zsh + +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: + record-lidar start [--seconds N] + record-lidar list-session + record-lidar list-recorded + record-lidar stop-session [--session NAME] + +Subcommands: + start Start recording all /livox* topics. + list-session List active remote ros2 bag recording processes. + list-recorded List recorded bags with topics, size and duration. + stop-session Stop remote recording screen sessions. + +Options (for start): + --seconds N Record for N seconds on remote via screen, then auto stop. +Options (for stop-session): + --session Stop a specific screen session. + --help, -h Show this help message. + +Without --seconds, start runs in foreground and blocks this terminal. +EOF +} + +record_action="" +record_seconds="" +record_session="" + +if (( $# == 0 )); then + usage + exit 1 +fi + +case "$1" in +start|list-session|list-recorded|stop-session) + record_action="$1" + shift + ;; +--help|-h) + usage + exit 0 + ;; +*) + echo "Error: unsupported subcommand '$1'." + usage + exit 1 + ;; +esac + +if [[ "${record_action}" == "start" ]]; then + while (( $# > 0 )); do + case "$1" in + --seconds) + if (( $# < 2 )); then + echo "Error: --seconds requires a value." + usage + exit 1 + fi + + if [[ ! "$2" =~ ^[1-9][0-9]*$ ]]; then + echo "Error: --seconds must be a positive integer." + usage + exit 1 + fi + + record_seconds="$2" + shift 2 + ;; + --help|-h) + usage + exit 0 + ;; + *) + echo "Error: unsupported argument '$1'." + usage + exit 1 + ;; + esac + done +else + if [[ "${record_action}" == "list-session" ]] || [[ "${record_action}" == "list-recorded" ]]; then + while (( $# > 0 )); do + case "$1" in + --help|-h) + usage + exit 0 + ;; + *) + echo "Error: subcommand '${record_action}' does not accept argument '$1'." + usage + exit 1 + ;; + esac + done + else + while (( $# > 0 )); do + case "$1" in + --session) + if (( $# < 2 )); then + echo "Error: --session requires a value." + usage + exit 1 + fi + record_session="$2" + shift 2 + ;; + --help|-h) + usage + exit 0 + ;; + *) + echo "Error: unsupported argument '$1'." + usage + exit 1 + ;; + esac + done + fi +fi + +remote_script='source ~/env_setup.bash && bash -s' + +if [[ "${record_action}" == "start" ]] && [[ -z "${record_seconds}" ]]; then + remote_ssh_cmd=(ssh -tt remote) +else + remote_ssh_cmd=(ssh remote) +fi + +"${remote_ssh_cmd[@]}" \ + "RECORD_ACTION='${record_action}' RECORD_SECONDS='${record_seconds}' RECORD_SESSION='${record_session}' bash -lc '${remote_script}'" <<'EOF' +set -euo pipefail + +readonly RECORD_ROOT="${HOME}/record-lidar" + +build_bag_dir() { + local timestamp + timestamp="$(date +%Y%m%d-%H%M%S)" + printf '%s/livox-%s' "${RECORD_ROOT}" "${timestamp}" +} + +collect_livox_topics() { + mapfile -t all_topics < <(ros2 topic list) + + livox_topics=() + for topic in "${all_topics[@]}"; do + if [[ "${topic}" == /livox* ]]; then + livox_topics+=("${topic}") + fi + done +} + +record_foreground() { + local bag_dir="$1" + shift + local topics=("$@") + local bag_pid="" + local status=0 + + cleanup_foreground() { + if [[ -n "${bag_pid}" ]] && kill -0 "${bag_pid}" >/dev/null 2>&1; then + kill -INT "${bag_pid}" >/dev/null 2>&1 || true + wait "${bag_pid}" >/dev/null 2>&1 || true + fi + } + + trap cleanup_foreground EXIT INT TERM HUP + + ros2 bag record -o "${bag_dir}" "${topics[@]}" & + bag_pid="$!" + + if ! wait "${bag_pid}"; then + status=$? + fi + + trap - EXIT INT TERM HUP + return "${status}" +} + +record_with_timeout() { + local seconds="$1" + local bag_dir="$2" + shift 2 + local topics=("$@") + + if ! command -v screen >/dev/null 2>&1; then + echo "Error: screen is not installed on remote." + exit 1 + fi + + local session_name + local record_cmd + local stop_cmd + + session_name="record-lidar-$(date +%Y%m%d-%H%M%S)" + printf -v record_cmd 'source ~/env_setup.bash && ros2 bag record -o %q' "${bag_dir}" + for topic in "${topics[@]}"; do + printf -v record_cmd '%s %q' "${record_cmd}" "${topic}" + done + + echo "Starting detached screen session: ${session_name}" + screen -dmS "${session_name}" bash -lc "${record_cmd}" + + printf -v stop_cmd 'sleep %q; screen -S %q -X stuff $'"'"'\003'"'"'; sleep 2; screen -S %q -X quit >/dev/null 2>&1 || true' \ + "${seconds}" "${session_name}" "${session_name}" + nohup bash -lc "${stop_cmd}" >/tmp/record-lidar-stop-${session_name}.log 2>&1 < /dev/null & + + echo "Will stop automatically after ${seconds}s." + echo "Recorder started in background; this terminal will not block." +} + +collect_record_sessions() { + local line + record_sessions=() + + while IFS= read -r line; do + if [[ "${line}" =~ [0-9]+\.(record-lidar-[^[:space:]]+) ]]; then + record_sessions+=("${BASH_REMATCH[1]}") + fi + done < <(screen -ls 2>/dev/null || true) +} + +list_recorded_bags() { + local -a bag_dirs + local -a sorted_bag_dirs + local -a topic_lines + local bag_dir + local bag_info + local bag_size + local bag_duration + local info_line + local topic_line + + shopt -s nullglob + bag_dirs=("${RECORD_ROOT}"/livox-*) + shopt -u nullglob + + if (( ${#bag_dirs[@]} == 0 )); then + echo "Recorded bags:" + echo " (none)" + return + fi + + mapfile -t sorted_bag_dirs < <(printf '%s\n' "${bag_dirs[@]}" | sort) + + echo "Recorded bags:" + for bag_dir in "${sorted_bag_dirs[@]}"; do + bag_info="$(ros2 bag info "${bag_dir}" 2>/dev/null || true)" + bag_size="unknown" + bag_duration="unknown" + topic_lines=() + + while IFS= read -r info_line; do + if [[ "${info_line}" == "Bag size:"* ]]; then + bag_size="${info_line#Bag size: }" + continue + fi + + if [[ "${info_line}" == "Duration:"* ]]; then + bag_duration="${info_line#Duration: }" + continue + fi + + if [[ "${info_line}" == *"Topic:"* ]]; then + topic_line="${info_line#*Topic: }" + topic_line="${topic_line%% | Type:*}" + topic_lines+=("${topic_line}") + fi + done <<< "${bag_info}" + + if [[ "${bag_size}" == "unknown" ]]; then + bag_size="$(du -sh "${bag_dir}" 2>/dev/null | awk '{print $1}')" + if [[ -z "${bag_size}" ]]; then + bag_size="unknown" + fi + fi + + echo " Path: ${bag_dir}" + echo " Size: ${bag_size}" + echo " Duration: ${bag_duration}" + echo " Topics:" + + if (( ${#topic_lines[@]} == 0 )); then + echo " (unavailable)" + else + printf ' %s\n' "${topic_lines[@]}" + fi + done +} + +stop_one_session() { + local session_name="$1" + + if ! screen -S "${session_name}" -Q select . >/dev/null 2>&1; then + echo "Session not found: ${session_name}" + return 1 + fi + + echo "Stopping session: ${session_name}" + screen -S "${session_name}" -X stuff $'\003' + sleep 2 + screen -S "${session_name}" -X quit >/dev/null 2>&1 || true + return 0 +} + +stop_recording() { + local target_session="${1:-}" + local stopped_count=0 + + if [[ -n "${target_session}" ]]; then + if stop_one_session "${target_session}"; then + echo "Stopped 1 session." + else + exit 1 + fi + return + fi + + collect_record_sessions + + if (( ${#record_sessions[@]} == 0 )); then + echo "No active record-lidar screen sessions found." + return + fi + + for session_name in "${record_sessions[@]}"; do + if stop_one_session "${session_name}"; then + ((stopped_count += 1)) + fi + done + + echo "Stopped ${stopped_count} session(s)." +} + +main() { + local action + local seconds + local bag_dir + + action="${RECORD_ACTION:-start}" + seconds="${RECORD_SECONDS:-}" + + case "${action}" in + start) + bag_dir="$(build_bag_dir)" + + mkdir -p "${RECORD_ROOT}" + collect_livox_topics + + if (( ${#livox_topics[@]} == 0 )); then + echo "Error: no /livox topics found on remote." + exit 1 + fi + + echo "Recording /livox topics:" + printf ' %s\n' "${livox_topics[@]}" + echo "Output directory: ${bag_dir}" + + if [[ -n "${seconds}" ]]; then + record_with_timeout "${seconds}" "${bag_dir}" "${livox_topics[@]}" + else + record_foreground "${bag_dir}" "${livox_topics[@]}" + fi + ;; + list-session) + local -a active_records + local line + + active_records=() + while IFS= read -r line; do + if [[ "${line}" == *"ros2 bag record"* ]] && [[ "${line}" != *"record-lidar list-session"* ]]; then + active_records+=("${line}") + fi + done < <(ps -eo pid,args) + + echo "Active remote recording processes:" + if (( ${#active_records[@]} > 0 )); then + printf ' %s\n' "${active_records[@]}" + else + echo " (none)" + fi + ;; + list-recorded) + list_recorded_bags + ;; + stop-session) + stop_recording "${RECORD_SESSION:-}" + ;; + *) + echo "Error: unsupported remote action '${action}'." + exit 1 + ;; + esac +} + +main +EOF diff --git a/.script/remote-status b/.script/remote-status new file mode 100755 index 00000000..1453152d --- /dev/null +++ b/.script/remote-status @@ -0,0 +1,42 @@ +#!/bin/zsh + +set -euo pipefail + +robot_status="/rmcs/service/robot_status" +referee_status="/rmcs_navigation/service/referee_status" + +ssh-remote bash -s -- "${robot_status}" "${referee_status}" <<'EOF' +set -euo pipefail + +source ~/env_setup.bash + +services=("$@") +all_services="$(ros2 service list || true)" + +call_status_service() { + local service="$1" + local raw + local msg + + if ! printf "%s\n" "$all_services" | grep -Fxq "$service"; then + printf "[WARN] service not found: %s\n\n" "$service" + return + fi + + printf "=== %s ===\n" "$service" + raw="$(ros2 service call "$service" std_srvs/srv/Trigger "{}" 2>&1 || true)" + msg="$(printf "%s\n" "$raw" | sed -n "s/.*message='\\(.*\\)'.*/\\1/p")" + + if [ -z "$msg" ]; then + printf "[WARN] failed to parse message: %s\n%s\n" "$service" "$raw" + else + printf "%b\n" "$msg" + fi + + printf "\n" +} + +for service in "${services[@]}"; do + call_status_service "$service" +done +EOF diff --git a/.script/save-map-once b/.script/save-map-once new file mode 100755 index 00000000..a7364d79 --- /dev/null +++ b/.script/save-map-once @@ -0,0 +1,45 @@ +#!/bin/zsh + +set -euo pipefail + +usage() { + cat <<'EOF' +Usage: save-map-once + + remote Trigger robot-side /save_pcd_map service via ssh remote. + local Trigger local container /save_pcd_map service. +EOF +} + +if (( $# != 1 )); then + usage + exit 1 +fi + +readonly mode="$1" +readonly service_call='ros2 service call /save_pcd_map std_srvs/srv/Trigger "{}"' + +call_local() { + bash -lc "source ~/env_setup.bash && ${service_call}" +} + +call_remote() { + ssh-remote "bash -lc 'source ~/env_setup.bash && ${service_call}'" +} + +case "$mode" in +remote) + call_remote + ;; +local) + call_local + ;; +--help|-h) + usage + ;; +*) + echo "Error: unsupported mode '$mode'." + usage + exit 1 + ;; +esac diff --git a/Dockerfile b/Dockerfile index ecce1882..d9fe2f27 100644 --- a/Dockerfile +++ b/Dockerfile @@ -29,8 +29,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libsuitesparse-dev \ libceres-dev \ ros-$ROS_DISTRO-rviz2 \ + ros-$ROS_DISTRO-navigation2 \ 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/* @@ -96,6 +96,7 @@ RUN mkdir -p /etc/apt/keyrings && \ > /etc/apt/sources.list.d/llvm.list && \ apt-get update && \ apt-get install -y --no-install-recommends \ + libomp-${LLVM_VERSION}-dev \ clang-${LLVM_VERSION} clangd-${LLVM_VERSION} clang-format-${LLVM_VERSION} clang-tidy-${LLVM_VERSION} \ lldb-${LLVM_VERSION} lld-${LLVM_VERSION} llvm-${LLVM_VERSION} && \ update-alternatives --install /usr/bin/clang clang /usr/bin/clang-${LLVM_VERSION} 50 && \ diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera deleted file mode 160000 index 815952fd..00000000 --- a/rmcs_ws/src/hikcamera +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml new file mode 100644 index 00000000..6d617a02 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -0,0 +1,151 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Sentry -> sentry_hardware + + - rmcs_core::referee::Status -> referee_status + - rmcs_core::referee::Command -> referee_command + + - rmcs_core::referee::command::Interaction -> referee_interaction + - rmcs_core::referee::command::interaction::Ui -> referee_ui + + - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + - rmcs_core::controller::gimbal::DualYawController -> dual_yaw_controller + - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + + - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + - rmcs_core::controller::shooting::HeatController -> heat_controller + - rmcs_core::controller::shooting::BulletFeederController17mm -> bullet_feeder_controller + - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + - rmcs_core::controller::pid::PidController -> bullet_feeder_velocity_pid_controller + + - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller + + # Alliance-Algorithm/rmcs-navigation + - rmcs_navigation::Navigation -> rmcs_navigation + +# The positive direction is the one that battery exists +sentry_hardware: + ros__parameters: + board_serial_top_board: "d4-2184" + board_serial_bottom_board: "d4-041d" + bottom_yaw_motor_zero_point: 44749 + top_yaw_motor_zero_point: 43751 + pitch_motor_zero_point: 1446 + left_front_zero_point: 5828 + left_back_zero_point: 5115 + right_back_zero_point: 2677 + right_front_zero_point: 6422 + +rmcs_navigation: + ros__parameters: + config_name: "rmul" + +gimbal_controller: + ros__parameters: + upper_limit: -0.39518 + lower_limit: 0.36 + navigation_scan_yaw_speed: 1.2 + navigation_scan_pitch_speed: 0.45 + +chassis_controller: + ros__parameters: + navigation_velocity_scale: 1.0 + +dual_yaw_controller: + ros__parameters: + top_yaw_angle_kp: 5.0 + top_yaw_angle_ki: 0.0 + top_yaw_angle_kd: 0.0 + top_yaw_velocity_kp: 7.0 + top_yaw_velocity_ki: 0.0 + top_yaw_velocity_kd: 0.0 + bottom_yaw_angle_kp: 8.6 + bottom_yaw_angle_ki: 0.0 + bottom_yaw_angle_kd: 0.0 + bottom_yaw_velocity_kp: 16.0 + bottom_yaw_velocity_ki: 0.1 + bottom_yaw_velocity_kd: 50.0 + bottom_yaw_velocity_integral_min: -10.0 + bottom_yaw_velocity_integral_max: +10.0 + bottom_yaw_velocity_output_min: -16.0 + bottom_yaw_velocity_output_max: +16.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 15.0 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 690.0 + - 690.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 10.0 + shot_frequency: 24.0 + safe_shot_frequency: 10.0 + eject_frequency: 15.0 + eject_time: 0.15 + deep_eject_frequency: 15.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.283 + ki: 0.0 + kd: 0.0 + +steering_wheel_controller: + ros__parameters: + mess: 22.0 + moment_of_inertia: 0.77852676 + vehicle_radius: 0.26870058 + wheel_radius: 0.055 + friction_coefficient: 0.5 + k1: 2.958580e+00 + k2: 3.082190e-03 + no_load_power: 11.37 + chassis_translation_kp: 8.0 + chassis_translation_ki: 0.0 + chassis_translation_kd: 0.0 diff --git a/rmcs_ws/src/rmcs_core/CMakeLists.txt b/rmcs_ws/src/rmcs_core/CMakeLists.txt index dd3c8579..86243959 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -8,20 +8,21 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-packed-bitfield-compat") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-O2 -Wall -Wextra -Wpedantic) + add_compile_options(-O2 -Wall -Wextra -Wpedantic) endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() + include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) -FetchContent_Declare( +fetchcontent_declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.0.0/librmcs-sdk-src-3.0.0.zip - URL_HASH SHA256=b39f51c21baacdcbf3f0176119b8850137a108b88a67e12395d37d89e5ef53e8 + URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.1.0b1/librmcs-sdk-src-3.1.0-beta.1.zip + URL_HASH SHA256=ec68a8b0e9441bd9e35b859139b3875c559944f7c0ecb4a1539bcdc75442fec4 DOWNLOAD_EXTRACT_TIMESTAMP TRUE ) -FetchContent_MakeAvailable(librmcs) +fetchcontent_makeavailable(librmcs) file(GLOB_RECURSE PROJECT_SOURCE CONFIGURE_DEPENDS ${PROJECT_SOURCE_DIR}/src/*.cpp diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 41074552..4312d334 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -11,6 +11,7 @@ rclcpp std_msgs + std_srvs pluginlib tf2 tf2_ros diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f7847151..e8dc5e5d 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -3,6 +3,7 @@ + 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 c9b9a584..ad39da8a 100644 --- a/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp +++ b/rmcs_ws/src/rmcs_core/src/broadcaster/value_broadcaster.cpp @@ -14,13 +14,12 @@ class ValueBroadcaster : Node{get_component_name()} { declare_parameter>("forward_list", std::vector{}); parameter_subscriber_ = std::make_unique(this); - parameter_callback_ = parameter_subscriber_->add_parameter_callback( + parameter_callback_ = parameter_subscriber_->add_parameter_callback( "forward_list", [this](const rclcpp::Parameter& para) { update_forward_list(para.as_string_array()); }); } - void before_pairing( - const std::map& output_map) override { + void before_pairing(const std::map& output_map) override { for (const auto& [name, type] : output_map) { if (type == typeid(double)) { forward_units_.emplace( @@ -52,7 +51,7 @@ class ValueBroadcaster "unsupported or the output does not exist.", name.c_str()); } else { - auto& unit = iter->second; + auto& unit = iter->second; unit->active = true; } } @@ -70,8 +69,8 @@ class ValueBroadcaster virtual ~BasicForwarderUnit() {} virtual void activate(rclcpp::Node* node) = 0; - virtual void update() = 0; - virtual void deactivate() = 0; + virtual void update() = 0; + virtual void deactivate() = 0; bool active; }; @@ -118,4 +117,4 @@ class ValueBroadcaster #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::broadcaster::ValueBroadcaster, rmcs_executor::Component) 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 da70a96f..9a2a6dab 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,4 +1,7 @@ +#include + #include +#include #include #include #include @@ -20,7 +23,9 @@ class ChassisController get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , following_velocity_controller_(7.0, 0.0, 0.0) { - following_velocity_controller_.output_max = angular_velocity_max; + navigation_velocity_scale_ = get_parameter_or("navigation_velocity_scale", 1.0); + + following_velocity_controller_.output_max = +angular_velocity_max; following_velocity_controller_.output_min = -angular_velocity_max; register_input("/remote/joystick/right", joystick_right_); @@ -31,6 +36,8 @@ class ChassisController register_input("/remote/mouse", mouse_); register_input("/remote/keyboard", keyboard_); register_input("/remote/rotary_knob", rotary_knob_); + register_input("/rmcs_navigation/chassis_velocity", navigation_command_velocity_, false); + register_input("/rmcs_navigation/rotate_chassis", navigation_rotate_chassis_, false); register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); @@ -58,8 +65,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +81,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -93,6 +100,13 @@ class ChassisController ? rmcs_msgs::ChassisMode::AUTO : rmcs_msgs::ChassisMode::STEP_DOWN; } + + // Navigation Mode + if (switch_right == Switch::UP && navigation_rotate_chassis_.ready()) { + const auto rotate = *navigation_rotate_chassis_; + mode = rotate ? ChassisMode::SPIN : ChassisMode::AUTO; + } + *mode_ = mode; } @@ -100,8 +114,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,35 +126,46 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); + + Eigen::Vector3d control_velocity; + control_velocity << translational_velocity, angular_velocity; - chassis_control_velocity_->vector << translational_velocity, angular_velocity; + chassis_control_velocity_->vector = control_velocity; } Eigen::Vector2d update_translational_velocity_control() { auto keyboard = *keyboard_; Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; - Eigen::Vector2d translational_velocity = - Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + auto translational_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} + * Eigen::Vector2d{*joystick_right_ + keyboard_move}; if (translational_velocity.norm() > 1.0) translational_velocity.normalize(); translational_velocity *= translational_velocity_max; - return translational_velocity; + // Navigation Control + auto nav_velocity_odom = Eigen::Vector2d{Eigen::Vector2d::Zero()}; + if (navigation_command_velocity_.ready()) { + auto raw_command = *navigation_command_velocity_; + if (std::isfinite(raw_command.x()) && std::isfinite(raw_command.y())) + nav_velocity_odom = *navigation_command_velocity_ * navigation_velocity_scale_; + } + auto nav_velocity = Eigen::Rotation2Dd{*gimbal_yaw_angle_} * nav_velocity_odom; + + return translational_velocity + nav_velocity; } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { case rmcs_msgs::ChassisMode::AUTO: break; case rmcs_msgs::ChassisMode::SPIN: { - angular_velocity = - 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + angular_velocity = (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); } break; case rmcs_msgs::ChassisMode::STEP_DOWN: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); @@ -171,7 +196,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -201,8 +226,8 @@ class ChassisController static constexpr double nan = std::numeric_limits::quiet_NaN(); // Maximum control velocities - static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double translational_velocity_max = 20.0; + static constexpr double angular_velocity_max = 2 * std::numbers::pi * 2.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,10 +239,11 @@ class ChassisController InputInterface rotary_knob_; 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(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + double navigation_velocity_scale_ = 1.0; OutputInterface chassis_angle_, chassis_control_angle_; OutputInterface mode_; @@ -225,10 +251,14 @@ class ChassisController pid::PidCalculator following_velocity_controller_; OutputInterface chassis_control_velocity_; + + // For Navigation + InputInterface navigation_command_velocity_; + InputInterface navigation_rotate_chassis_; }; } // 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::ChassisController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index 2d084f8b..3186ef62 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -51,9 +51,9 @@ class ChassisPowerController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; - auto rotary_knob = *rotary_knob_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + auto rotary_knob = *rotary_knob_; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { @@ -74,8 +74,8 @@ class ChassisPowerController // charging_power_limit = constexpr double buffer_energy_control_line = 120; // = referee + excess - constexpr double buffer_energy_base_line = 30; // = referee - constexpr double buffer_energy_dead_line = 0; // = 0 + constexpr double buffer_energy_base_line = 30; // = referee + constexpr double buffer_energy_dead_line = 0; // = 0 *supercap_charge_power_limit_ = *chassis_power_limit_referee_ @@ -91,8 +91,8 @@ class ChassisPowerController } void reset_power_control() { - virtual_buffer_energy_ = virtual_buffer_energy_limit_; - boost_mode_ = false; + virtual_buffer_energy_ = virtual_buffer_energy_limit_; + boost_mode_ = false; *chassis_control_power_limit_ = 0.0; } @@ -117,8 +117,8 @@ class ChassisPowerController // chassis_control_power_limit = constexpr double supercap_voltage_control_line = 12.5; // = supercap - constexpr double supercap_voltage_base_line = 12.0; // = referee - power_limit = *chassis_power_limit_referee_ + constexpr double supercap_voltage_base_line = 12.0; // = referee + power_limit = *chassis_power_limit_referee_ + (power_limit - *chassis_power_limit_referee_) * std::clamp( (*supercap_voltage_ - supercap_voltage_base_line) 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 index 83af9894..189b82f2 100644 --- 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 @@ -35,7 +35,11 @@ class SteeringWheelController , 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_translational_velocity_pid_{ + get_parameter_or("chassis_translation_kp", 0.0), + get_parameter_or("chassis_translation_ki", 0.0), + get_parameter_or("chassis_translation_kd", 0.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) @@ -501,4 +505,4 @@ class SteeringWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::SteeringWheelController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::chassis::SteeringWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp index 80d7a72e..4e72d1af 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/dual_yaw_controller.cpp @@ -1,4 +1,4 @@ - +#include #include #include @@ -38,6 +38,7 @@ class DualYawController register_input("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); register_input("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_); + register_input("/referee/chassis/output_status", chassis_output_status_, false); register_input("/gimbal/yaw/control_angle_error", control_angle_error_); register_input("/gimbal/yaw/control_angle_shift", control_angle_shift_, false); @@ -45,11 +46,12 @@ class DualYawController register_output("/gimbal/top_yaw/control_torque", top_yaw_control_torque_, 0.0); register_output("/gimbal/bottom_yaw/control_torque", bottom_yaw_control_torque_, 0.0); - register_output("/gimbal/top_yaw/control_angle", top_yaw_control_torque_, 0.0); - register_output("/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_torque_, 0.0); + register_output("/gimbal/top_yaw/control_angle", top_yaw_control_angle_, nan_); + register_output( + "/gimbal/bottom_yaw/control_angle_shift", bottom_yaw_control_angle_shift_, nan_); - status_component_ = - create_partner_component(get_component_name() + "_status"); + register_output("/gimbal/yaw/angle", yaw_angle_, 0.0); + register_output("/gimbal/yaw/velocity", yaw_velocity_, 0.0); } void before_updating() override { @@ -58,15 +60,32 @@ class DualYawController get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_shift\", set to NaN."); control_angle_shift_.bind_directly(nan_); } + if (!chassis_output_status_.ready()) { + RCLCPP_WARN( + get_logger(), + "Failed to fetch \"/referee/chassis/output_status\", fallback to enabled."); + chassis_output_status_.bind_directly(true); + } } void update() override { - if (std::isnan(*control_angle_error_)) { + if (std::isnan(*control_angle_error_) || !*chassis_output_status_) { + reset_pid_states(); *top_yaw_control_torque_ = nan_; *bottom_yaw_control_torque_ = nan_; } else { - *top_yaw_control_torque_ = top_yaw_velocity_pid_.update( - top_yaw_angle_pid_.update(*control_angle_error_) - *gimbal_yaw_velocity_imu_); + + /// @FIXME: + /// The implement of dual yaw controlling is not completed + /// Let it stable as a common gimbal + { + auto true_angle = std::remainder(*top_yaw_angle_, 2 * std::numbers::pi); + + const auto velocity = top_yaw_angle_pid_.update(-true_angle); + const auto torque = top_yaw_velocity_pid_.update(velocity); + + *top_yaw_control_torque_ = torque; + } *bottom_yaw_control_torque_ = bottom_yaw_velocity_pid_.update( bottom_yaw_angle_pid_.update(bottom_yaw_control_error()) @@ -80,24 +99,38 @@ class DualYawController *top_yaw_control_angle_ = 0.0; *bottom_yaw_control_angle_shift_ = *control_angle_shift_; } + + double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_; + if (yaw_angle < 0) + yaw_angle += 2 * std::numbers::pi; + else if (yaw_angle > 2 * std::numbers::pi) + yaw_angle -= 2 * std::numbers::pi; + *yaw_angle_ = yaw_angle; + + *yaw_velocity_ = *top_yaw_velocity_ + *bottom_yaw_velocity_; } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); double bottom_yaw_control_error() { - double err = *top_yaw_angle_ + *control_angle_error_; - if (err > std::numbers::pi) - err -= 2 * std::numbers::pi; - return err; + return std::remainder(*top_yaw_angle_ + *control_angle_error_, 2 * std::numbers::pi); } double bottom_yaw_velocity_imu() { return *chassis_yaw_velocity_imu_ + *bottom_yaw_velocity_; } + void reset_pid_states() { + top_yaw_angle_pid_.reset(); + top_yaw_velocity_pid_.reset(); + bottom_yaw_angle_pid_.reset(); + bottom_yaw_velocity_pid_.reset(); + } + InputInterface top_yaw_angle_, top_yaw_velocity_; InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; InputInterface gimbal_yaw_velocity_imu_, chassis_yaw_velocity_imu_; + InputInterface chassis_output_status_; InputInterface control_angle_error_, control_angle_shift_; @@ -110,39 +143,10 @@ class DualYawController OutputInterface top_yaw_control_angle_; OutputInterface bottom_yaw_control_angle_shift_; - class DualYawStatus : public rmcs_executor::Component { - public: - explicit DualYawStatus() { - register_input("/gimbal/top_yaw/angle", top_yaw_angle_); - register_input("/gimbal/top_yaw/velocity", top_yaw_velocity_); - register_input("/gimbal/bottom_yaw/angle", bottom_yaw_angle_); - register_input("/gimbal/bottom_yaw/velocity", bottom_yaw_velocity_); - - register_output("/gimbal/yaw/angle", yaw_angle_, 0.0); - register_output("/gimbal/yaw/velocity", yaw_velocity_, 0.0); - } - - void update() override { - double yaw_angle = *top_yaw_angle_ + *bottom_yaw_angle_; - if (yaw_angle < 0) - yaw_angle += 2 * std::numbers::pi; - else if (yaw_angle > 2 * std::numbers::pi) - yaw_angle -= 2 * std::numbers::pi; - *yaw_angle_ = yaw_angle; - - *yaw_velocity_ = *top_yaw_velocity_ + *bottom_yaw_velocity_; - } - - private: - InputInterface top_yaw_angle_, top_yaw_velocity_; - InputInterface bottom_yaw_angle_, bottom_yaw_velocity_; - - OutputInterface yaw_angle_, yaw_velocity_; - }; - std::shared_ptr status_component_; + OutputInterface yaw_angle_, yaw_velocity_; }; } // namespace rmcs_core::controller::gimbal #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::DualYawController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp index d27704d5..855a2548 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/simple_gimbal_controller.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -20,9 +21,15 @@ class SimpleGimbalController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , two_axis_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { + , upper_limit_(get_parameter("upper_limit").as_double()) + , lower_limit_(get_parameter("lower_limit").as_double()) + , two_axis_gimbal_solver(*this, upper_limit_, lower_limit_) { + + navigation_scan_yaw_speed_ = get_parameter_or("navigation_scan_yaw_speed", 0.0); + navigation_scan_pitch_speed_ = + std::abs(get_parameter_or("navigation_scan_pitch_speed", 0.0)); + navigation_scan_pitch_upper_ = upper_limit_ * 0.5; + navigation_scan_pitch_lower_ = lower_limit_ * 0.5; register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -31,9 +38,14 @@ class SimpleGimbalController register_input("/remote/mouse", mouse_); register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); + + // Only For Navigation, Not Required + register_input("/rmcs_navigation/gimbal_velocity", navigation_gimbal_velocity_, false); + register_input("/rmcs_navigation/gimbal_scanning", navigation_gimbal_scanning_, false); } void update() override { @@ -69,12 +81,31 @@ class SimpleGimbalController double pitch_shift = -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + // Navigation Control + { + // Navigation Direction + if (navigation_gimbal_velocity_.ready()) { + auto yaw_speed = navigation_gimbal_velocity_->x(); + if (std::isfinite(yaw_speed)) + yaw_shift += yaw_speed * control_dt_; + + auto pitch_speed = navigation_gimbal_velocity_->y(); + if (std::isfinite(pitch_speed)) + pitch_shift += pitch_speed * control_dt_; + } + + // Scanning Mode + if (switch_left != Switch::DOWN && switch_right == Switch::UP) + update_navigation_gimbal_scanning(yaw_shift, pitch_shift); + } + return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); } private: static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double control_dt_ = 1e-3; InputInterface joystick_left_; InputInterface switch_right_; @@ -83,10 +114,58 @@ class SimpleGimbalController InputInterface mouse_; InputInterface auto_aim_control_direction_; + InputInterface gimbal_pitch_angle_; + + const double upper_limit_; + const double lower_limit_; TwoAxisGimbalSolver two_axis_gimbal_solver; OutputInterface yaw_angle_error_, pitch_angle_error_; + + // For Navigation + InputInterface navigation_gimbal_velocity_; + InputInterface navigation_gimbal_scanning_; + + bool last_navigation_gimbal_scanning_ = false; + double navigation_scan_yaw_speed_ = 0.0; + double navigation_scan_pitch_speed_ = 0.0; + double navigation_scan_pitch_upper_ = 0.0; + double navigation_scan_pitch_lower_ = 0.0; + double scanning_pitch_direction_ = +1.0; + + void update_navigation_gimbal_scanning(double& yaw_shift, double& pitch_shift) { + const auto scanning_enabled = + navigation_gimbal_scanning_.ready() && *navigation_gimbal_scanning_; + if (!scanning_enabled) { + last_navigation_gimbal_scanning_ = false; + scanning_pitch_direction_ = +1.0; + return; + } + + auto current_pitch = *gimbal_pitch_angle_; + auto middle_pitch = (navigation_scan_pitch_upper_ + navigation_scan_pitch_lower_) * 0.5; + + if (!last_navigation_gimbal_scanning_) { + scanning_pitch_direction_ = current_pitch >= middle_pitch ? -1.0 : +1.0; + } + + constexpr double kBoundaryTolerance = 1e-3; + if (current_pitch > std::numbers::pi) { + current_pitch -= 2 * std::numbers::pi; + } + + if (current_pitch <= navigation_scan_pitch_upper_ + kBoundaryTolerance) { + scanning_pitch_direction_ = +1.0; + } else if (current_pitch >= navigation_scan_pitch_lower_ - kBoundaryTolerance) { + scanning_pitch_direction_ = -1.0; + } + + yaw_shift += navigation_scan_yaw_speed_ * control_dt_; + pitch_shift += scanning_pitch_direction_ * navigation_scan_pitch_speed_ * control_dt_; + + last_navigation_gimbal_scanning_ = true; + } }; } // namespace rmcs_core::controller::gimbal @@ -94,4 +173,4 @@ class SimpleGimbalController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::gimbal::SimpleGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp index 5d937aae..3e1d44ae 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/two_axis_gimbal_solver.hpp @@ -45,7 +45,7 @@ class TwoAxisGimbalSolver { PitchLink::DirectionVector update(TwoAxisGimbalSolver& super) const override { auto odom_dir = fast_tf::cast( PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *super.tf_); - if (odom_dir->x() == 0 || odom_dir->y() == 0) + if (std::abs(odom_dir->x()) < 1e-6 && std::abs(odom_dir->y()) < 1e-6) return {}; super.control_enabled_ = true; @@ -136,7 +136,7 @@ class TwoAxisGimbalSolver { auto yaw_axis = fast_tf::cast(yaw_axis_filtered_, *tf_); pitch = {yaw_axis->z(), yaw_axis->x()}; - pitch.normalized(); + pitch.normalize(); const auto& [x, y, z] = *dir; dir_yaw_link = {x * pitch.x() - z * pitch.y(), y, x * pitch.y() + z * pitch.x()}; @@ -195,4 +195,4 @@ class TwoAxisGimbalSolver { OdomImu::DirectionVector control_direction_; }; -} // namespace rmcs_core::controller::gimbal \ No newline at end of file +} // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp index a83dc5f4..5bd2dffa 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/friction_wheel_controller.cpp @@ -219,4 +219,4 @@ class FrictionWheelController #include PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) \ No newline at end of file + rmcs_core::controller::shooting::FrictionWheelController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp index 5500b15c..89162a77 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/shooting_recorder.cpp @@ -42,13 +42,13 @@ class ShootingRecorder using namespace std::chrono; auto now = high_resolution_clock::now(); - auto ms = duration_cast(now.time_since_epoch()).count(); + auto ms = duration_cast(now.time_since_epoch()).count(); auto file = fmt::format("/robot_shoot/{}.log", ms); log_stream_.open(file); } - ~ShootingRecorder() { log_stream_.close(); } + ~ShootingRecorder() override { log_stream_.close(); } void update() override { @@ -65,7 +65,7 @@ class ShootingRecorder break; } - auto log_text = std::string{}; + auto log_text = std::string{}; auto timestamp = timestamp_to_string(*shoot_timestamp_); if (friction_wheel_count_ == 2) { @@ -108,14 +108,14 @@ class ShootingRecorder private: static std::string timestamp_to_string(double timestamp) { - auto time = static_cast(timestamp); + auto time = static_cast(timestamp); auto local_time = std::localtime(&time); char buffer[100]; std::strftime(buffer, sizeof(buffer), "%Y-%m-%d %H:%M:%S", local_time); double fractional_seconds = timestamp - std::floor(timestamp); - int milliseconds = static_cast(fractional_seconds * 1000); + int milliseconds = static_cast(fractional_seconds * 1000); char result[150]; std::snprintf(result, sizeof(result), "%s.%03d", buffer, milliseconds); @@ -128,4 +128,4 @@ class ShootingRecorder #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::shooting::ShootingRecorder, rmcs_executor::Component) 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 bb3f1c93..d1c56cbf 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 @@ -200,6 +200,8 @@ class DjiMotor { return encoder_zero_point_; } + int last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp index 68b2e4f6..c11061c2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/lk_motor.hpp @@ -203,6 +203,8 @@ class LkMotor { return encoder_zero_point_; } + int64_t last_raw_angle() const { return last_raw_angle_; } + double angle() const { return angle_; } double velocity() const { return velocity_; } double torque() const { return torque_; } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp new file mode 100644 index 00000000..a82779ab --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/sentry.cpp @@ -0,0 +1,571 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/can_packet.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" +#include "hardware/device/supercap.hpp" + +namespace rmcs_core::hardware { + +namespace { + +double wrap_single_turn(double angle) { + return std::remainder(angle, 2.0 * std::numbers::pi_v); +} + +Eigen::Quaterniond top_board_pitch_compensation(double pitch_angle) { + return Eigen::Quaterniond{Eigen::AngleAxisd{-pitch_angle, Eigen::Vector3d::UnitY()}}; +} + +} // namespace + +class Sentry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Sentry() + : Node( + get_component_name(), + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) + , command_component_( + create_partner_component(get_component_name() + "_command", *this)) { + register_output("/tf", tf_); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + steers_calibrate_subscription_ = create_subscription( + "/steers/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + steers_calibrate_subscription_callback(std::move(msg)); + }); + + // For command: remote-status + status_service_ = Node::create_service( + "/rmcs/service/robot_status", + [this]( + const std_srvs::srv::Trigger::Request::SharedPtr&, + const std_srvs::srv::Trigger::Response::SharedPtr& response) { + status_service_callback(response); + }); + + top_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_top_board").as_string()); + + bottom_board_ = std::make_unique( + *this, *command_component_, get_parameter("board_serial_bottom_board").as_string()); + + tf_->set_transform( + Eigen::Translation3d{0.06603, 0.0, 0.082}); + } + + Sentry(const Sentry&) = delete; + Sentry& operator=(const Sentry&) = delete; + Sentry(Sentry&&) = delete; + Sentry& operator=(Sentry&&) = delete; + + ~Sentry() override = default; + + void update() override { + top_board_->update(); + bottom_board_->update(); + } + + void command_update() { + top_board_->command_update(); + bottom_board_->command_update(); + } + +private: + auto status_service_callback(const std::shared_ptr& response) + -> void { + response->success = true; + + auto feedback_message = std::ostringstream{}; + auto text = [&](std::format_string format, Args&&... args) { + std::println(feedback_message, format, std::forward(args)...); + }; + + text("Gimbal Status"); + text("- Bottom Yaw: {}", bottom_board_->gimbal_bottom_yaw_motor_.last_raw_angle()); + text("- Top Yaw: {}", top_board_->gimbal_top_yaw_motor_.last_raw_angle()); + text("- Pitch Angle: {}", top_board_->gimbal_pitch_motor_.last_raw_angle()); + + text("Chassis Status"); + constexpr auto position = + std::array{"right back", "right front", "left front", "left back"}; + constexpr auto max_length = + std::ranges::max_element(position, {}, &std::string_view::size)->size(); + + for (auto&& [index, motor] : + std::views::zip(position, bottom_board_->chassis_steer_motors_)) { + text("- {:{}}: {}", index, max_length, motor.last_raw_angle()); + } + + response->message = feedback_message.str(); + } + + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New yaw offset: %ld", + bottom_board_->gimbal_bottom_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New top yaw offset: %ld", + top_board_->gimbal_top_yaw_motor_.calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[gimbal calibration] New pitch offset: %ld", + top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + } + + void steers_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + get_logger(), "[steer calibration] New left front offset: %d", + bottom_board_->chassis_steer_motors_[2].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New left back offset: %d", + bottom_board_->chassis_steer_motors_[3].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New right back offset: %d", + bottom_board_->chassis_steer_motors_[0].calibrate_zero_point()); + RCLCPP_INFO( + get_logger(), "[steer calibration] New right front offset: %d", + bottom_board_->chassis_steer_motors_[1].calibrate_zero_point()); + } + + class SentryCommand : public rmcs_executor::Component { + public: + explicit SentryCommand(Sentry& sentry) + : sentry(sentry) {} + + void update() override { sentry.command_update(); } + + Sentry& sentry; + }; + + class TopBoard final : private librmcs::agent::CBoard { + public: + friend class Sentry; + explicit TopBoard( + Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , tf_(sentry.tf_) + , bmi088_(1000, 0.2, 0.0) + , gimbal_pitch_motor_(sentry, sentry_command, "/gimbal/pitch") + , gimbal_top_yaw_motor_(sentry, sentry_command, "/gimbal/top_yaw") + , gimbal_bullet_feeder_(sentry, sentry_command, "/gimbal/bullet_feeder") + , gimbal_left_friction_(sentry, sentry_command, "/gimbal/left_friction") + , gimbal_right_friction_(sentry, sentry_command, "/gimbal/right_friction") { + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("pitch_motor_zero_point").as_int()))); + + gimbal_top_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(sentry.get_parameter("top_yaw_motor_zero_point").as_int()))); + + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .enable_multi_turn_angle() + .set_reversed() + .set_reduction_ratio(19 * 2)); + + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(1.) + .set_reversed()); + + sentry.register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_bmi088_); + sentry.register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_bmi088_); + + bmi088_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise + // use a matrix. + + // Eigen::AngleAxisd pitch_link_to_bmi088_link{ + // std::numbers::pi / 2, Eigen::Vector3d::UnitZ()}; + // Eigen::Vector3d mapping = pitch_link_to_bmi088_link * + // Eigen::Vector3d{1, 2, 3}; std::cout << mapping << std::endl; + + return std::make_tuple(-x, -y, z); + }); + } + + TopBoard(const TopBoard&) = delete; + TopBoard& operator=(const TopBoard&) = delete; + TopBoard(TopBoard&&) = delete; + TopBoard& operator=(TopBoard&&) = delete; + + ~TopBoard() override = default; + + void update() { + gimbal_top_yaw_motor_.update_status(); + gimbal_pitch_motor_.update_status(); + + const auto pitch_angle = wrap_single_turn(gimbal_pitch_motor_.angle()); + + bmi088_.update_status(); + const Eigen::Quaterniond gimbal_bmi088_pose{ + bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + + tf_->set_transform( + top_board_pitch_compensation(pitch_angle) * gimbal_bmi088_pose.conjugate()); + + *gimbal_yaw_velocity_bmi088_ = bmi088_.gz(); + *gimbal_pitch_velocity_bmi088_ = bmi088_.gy(); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + tf_->set_state( + gimbal_pitch_motor_.angle()); + // tf_->set_state( + // gimbal_top_yaw_motor_.angle()); + } + + void command_update() { + auto builder = start_transmit(); + + builder.can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + gimbal_right_friction_.generate_command(), + gimbal_left_friction_.generate_command(), + device::CanPacket8::PaddingQuarter{}, + gimbal_bullet_feeder_.generate_command(), + } + .as_bytes(), + }); + + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_top_yaw_motor_.generate_torque_command().as_bytes(), + }); + + builder.can2_transmit({ + .can_id = 0x141, + .can_data = gimbal_pitch_motor_.generate_velocity_command().as_bytes(), + }); + } + + private: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) { + gimbal_left_friction_.store_status(data.can_data); + } else if (can_id == 0x201) { + gimbal_right_friction_.store_status(data.can_data); + } else if (can_id == 0x204) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (can_id == 0x141) { + gimbal_top_yaw_motor_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x141) + gimbal_pitch_motor_.store_status(data.can_data); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + bmi088_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + bmi088_.store_gyroscope_status(data.x, data.y, data.z); + } + + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_bmi088_; + OutputInterface gimbal_pitch_velocity_bmi088_; + + device::Bmi088 bmi088_; + device::LkMotor gimbal_pitch_motor_; + device::LkMotor gimbal_top_yaw_motor_; + device::DjiMotor gimbal_bullet_feeder_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + }; + + class BottomBoard final : private librmcs::agent::CBoard { + public: + friend class Sentry; + + explicit BottomBoard( + Sentry& sentry, SentryCommand& sentry_command, std::string_view board_serial = {}) + : librmcs::agent::CBoard(board_serial) + , imu_(1000, 0.2, 0.0) + , tf_(sentry.tf_) + , dr16_(sentry) + , gimbal_bottom_yaw_motor_(sentry, sentry_command, "/gimbal/bottom_yaw") + , chassis_wheel_motors_( + {sentry, sentry_command, "/chassis/left_front_wheel"}, + {sentry, sentry_command, "/chassis/left_back_wheel"}, + {sentry, sentry_command, "/chassis/right_back_wheel"}, + {sentry, sentry_command, "/chassis/right_front_wheel"}) + , chassis_steer_motors_( + {sentry, sentry_command, "/chassis/left_front_steering"}, + {sentry, sentry_command, "/chassis/left_back_steering"}, + {sentry, sentry_command, "/chassis/right_back_steering"}, + {sentry, sentry_command, "/chassis/right_front_steering"}) + , supercap_(sentry, sentry_command) { + sentry.register_output("/referee/serial", referee_serial_); + + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit( + {.uart_data = std::span{buffer, size}}); + return size; + }; + + gimbal_bottom_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG6012Ei8} + .set_reversed() + .set_encoder_zero_point( + static_cast( + sentry.get_parameter("bottom_yaw_motor_zero_point").as_int()))); + + for (auto& motor : chassis_wheel_motors_) + motor.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reduction_ratio(11.) + .enable_multi_turn_angle() + .set_reversed()); + chassis_steer_motors_[2].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("left_front_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[3].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("left_back_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[0].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("right_back_zero_point").as_int())) + .enable_multi_turn_angle()); + chassis_steer_motors_[1].configure( + device::DjiMotor::Config{device::DjiMotor::Type::kGM6020} + .set_reversed() + .set_encoder_zero_point( + static_cast(sentry.get_parameter("right_front_zero_point").as_int())) + .enable_multi_turn_angle()); + sentry.register_output("/chassis/yaw/velocity_imu", chassis_yaw_velocity_imu_, 0); + } + + BottomBoard(const BottomBoard&) = delete; + BottomBoard& operator=(const BottomBoard&) = delete; + BottomBoard(BottomBoard&&) = delete; + BottomBoard& operator=(BottomBoard&&) = delete; + + ~BottomBoard() override = default; + + void update() { + imu_.update_status(); + *chassis_yaw_velocity_imu_ = imu_.gz(); + supercap_.update_status(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + for (auto& motor : chassis_steer_motors_) + motor.update_status(); + + dr16_.update_status(); + gimbal_bottom_yaw_motor_.update_status(); + tf_->set_state( + gimbal_bottom_yaw_motor_.angle()); + } + + void command_update() { + auto builder = start_transmit(); + builder.can1_transmit({ + .can_id = 0x141, + .can_data = gimbal_bottom_yaw_motor_.generate_command().as_bytes(), + }); + + if (can_transmission_mode_) { + builder + .can1_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + chassis_wheel_motors_[1].generate_command(), + chassis_wheel_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can2_transmit({ + .can_id = 0x200, + .can_data = + device::CanPacket8{ + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[2].generate_command(), + device::CanPacket8::PaddingQuarter{}, + chassis_wheel_motors_[3].generate_command(), + } + .as_bytes(), + }); + } else { + builder + .can1_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steer_motors_[1].generate_command(), + chassis_steer_motors_[0].generate_command(), + device::CanPacket8::PaddingQuarter{}, + device::CanPacket8::PaddingQuarter{}, + } + .as_bytes(), + }) + .can2_transmit({ + .can_id = 0x1FE, + .can_data = + device::CanPacket8{ + chassis_steer_motors_[2].generate_command(), + chassis_steer_motors_[3].generate_command(), + device::CanPacket8::PaddingQuarter{}, + supercap_.generate_command(), + } + .as_bytes(), + }); + } + can_transmission_mode_ = !can_transmission_mode_; + } + + private: + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x201) + chassis_wheel_motors_[1].store_status(data.can_data); + else if (can_id == 0x202) + chassis_wheel_motors_[0].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[1].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[0].store_status(data.can_data); + else if (can_id == 0x141) + gimbal_bottom_yaw_motor_.store_status(data.can_data); + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission) [[unlikely]] + return; + auto can_id = data.can_id; + if (can_id == 0x202) + chassis_wheel_motors_[2].store_status(data.can_data); + else if (can_id == 0x204) + chassis_wheel_motors_[3].store_status(data.can_data); + else if (can_id == 0x205) + chassis_steer_motors_[2].store_status(data.can_data); + else if (can_id == 0x206) + chassis_steer_motors_[3].store_status(data.can_data); + else if (can_id == 0x300) + supercap_.store_status(data.can_data); + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const auto* uart_data = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&uart_data](std::byte* storage) noexcept { *storage = *uart_data++; }, + data.uart_data.size()); + } + + void accelerometer_receive_callback( + const librmcs::data::AccelerometerDataView& data) override { + imu_.store_accelerometer_status(data.x, data.y, data.z); + } + + void gyroscope_receive_callback(const librmcs::data::GyroscopeDataView& data) override { + imu_.store_gyroscope_status(data.x, data.y, data.z); + } + + bool can_transmission_mode_ = true; + device::Bmi088 imu_; + OutputInterface& tf_; + + device::Dr16 dr16_; + device::LkMotor gimbal_bottom_yaw_motor_; + device::DjiMotor chassis_wheel_motors_[4]; + device::DjiMotor chassis_steer_motors_[4]; + device::Supercap supercap_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + OutputInterface chassis_yaw_velocity_imu_; + }; + + OutputInterface tf_; + + std::shared_ptr command_component_; + std::unique_ptr top_board_; + std::unique_ptr bottom_board_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + rclcpp::Subscription::SharedPtr steers_calibrate_subscription_; + + std::shared_ptr> status_service_; +}; + +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Sentry, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index 7daf4633..106f48e6 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -19,11 +19,17 @@ class Status , public rclcpp::Node { public: Status() - : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} , logger_(get_logger()) { register_input("/referee/serial", serial_); register_output("/referee/game/stage", game_stage_, rmcs_msgs::GameStage::UNKNOWN); + register_output("/referee/game/red_score", red_score_, 0); + register_output("/referee/game/blue_score", blue_score_, 0); + register_output("/referee/game/current_round", current_round_, 0); + register_output("/referee/game/total_rounds", total_rounds_, 0); register_output("/referee/id", robot_id_, rmcs_msgs::RobotId::UNKNOWN); register_output("/referee/shooter/cooling", robot_shooter_cooling_, 0); @@ -34,6 +40,7 @@ class Status register_output("/referee/chassis/output_status", chassis_output_status_, false); register_output("/referee/robots/hp", robots_hp_); + register_output("/referee/current_hp", robot_current_hp_); register_output("/referee/shooter/bullet_allowance", robot_bullet_allowance_, false); register_output( "/referee/shooter/42mm_bullet_allowance", robot_42mm_bullet_allowance_, false); @@ -119,6 +126,11 @@ class Status auto& data = reinterpret_cast(frame_.body.data); *game_stage_ = static_cast(data.game_stage); + *red_score_ = data.red_score; + *blue_score_ = data.blue_score; + *current_round_ = data.current_round; + *total_rounds_ = data.total_rounds; + if (*game_stage_ == rmcs_msgs::GameStage::STARTED) game_status_watchdog_.reset(30'000); else @@ -135,6 +147,7 @@ class Status auto& data = reinterpret_cast(frame_.body.data); + *robot_current_hp_ = data.current_hp; *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; @@ -191,6 +204,8 @@ class Status rmcs_utility::TickTimer game_status_watchdog_; OutputInterface game_stage_; + OutputInterface red_score_, blue_score_; + OutputInterface current_round_, total_rounds_; rmcs_utility::TickTimer robot_status_watchdog_; OutputInterface robot_id_; @@ -203,6 +218,7 @@ class Status OutputInterface robot_buffer_energy_; OutputInterface robots_hp_; + OutputInterface robot_current_hp_; OutputInterface robot_bullet_allowance_; OutputInterface robot_42mm_bullet_allowance_; @@ -214,4 +230,4 @@ class Status #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Status, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::referee::Status, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index 5cc1129f..8a78d9ac 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -9,6 +9,11 @@ struct __attribute__((packed)) GameStatus { uint8_t game_stage : 4; uint16_t stage_remain_time; uint64_t sync_timestamp; + + uint32_t red_score; + uint32_t blue_score; + uint8_t current_round; + uint8_t total_rounds; }; struct __attribute__((packed)) GameRobotHp { diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp index 622a171d..c16ac02e 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/game_stage.hpp @@ -5,13 +5,25 @@ namespace rmcs_msgs { enum class GameStage : uint8_t { - NOT_START = 0, - PREPARATION = 1, + NOT_START = 0, + PREPARATION = 1, REFEREE_CHECK = 2, - COUNTDOWN = 3, - STARTED = 4, - SETTLING = 5, - UNKNOWN = UINT8_MAX, + COUNTDOWN = 3, + STARTED = 4, + SETTLING = 5, + UNKNOWN = UINT8_MAX, }; +constexpr auto to_string(GameStage stage) noexcept { + switch (stage) { + case GameStage::NOT_START: return "NOT_START"; + case GameStage::PREPARATION: return "PREPARATION"; + case GameStage::REFEREE_CHECK: return "REFEREE_CHECK"; + case GameStage::COUNTDOWN: return "COUNTDOWN"; + case GameStage::STARTED: return "STARTED"; + case GameStage::SETTLING: return "SETTLING"; + case GameStage::UNKNOWN: return "UNKNOWN"; + } + return "UNREACHABLE"; +} -} // namespace rmcs_msgs \ No newline at end of file +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robots_hp.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robots_hp.hpp new file mode 100644 index 00000000..d1092907 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/robots_hp.hpp @@ -0,0 +1,25 @@ +#pragma once +#include + +namespace rmcs_msgs { + +struct RobotsHp { + std::uint16_t red_1; + std::uint16_t red_2; + std::uint16_t red_3; + std::uint16_t red_4; + std::uint16_t red_5; + std::uint16_t red_7; + std::uint16_t red_outpost; + std::uint16_t red_base; + std::uint16_t blue_1; + std::uint16_t blue_2; + std::uint16_t blue_3; + std::uint16_t blue_4; + std::uint16_t blue_5; + std::uint16_t blue_7; + std::uint16_t blue_outpost; + std::uint16_t blue_base; +}; + +} // namespace rmcs_msgs diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp index a198e5a8..e76fc3a3 100644 --- a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/serial_interface.hpp @@ -7,7 +7,7 @@ namespace rmcs_msgs { struct SerialInterface { std::function read; - std::function write; + std::function write; }; -} \ No newline at end of file +} // namespace rmcs_msgs