diff --git a/.gitmodules b/.gitmodules index d6298295..98d15f11 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,6 +4,13 @@ [submodule "rmcs_ws/src/hikcamera"] path = rmcs_ws/src/hikcamera url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git + branch =2.0 [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial url = https://github.com/Alliance-Algorithm/ros2-serial.git +[submodule "rmcs_ws/src/odin_ros_driver"] + path = rmcs_ws/src/odin_ros_driver + url = https://github.com/Alliance-Algorithm/odin_ros_driver.git +[submodule "rmcs_ws/src/rmcs_auto_aim_v2"] + path = rmcs_ws/src/rmcs_auto_aim_v2 + url = https://github.com/Alliance-Algorithm/rmcs_auto_aim_v2.git diff --git a/.script/build-rmcs b/.script/build-rmcs index 2c24a7ed..8769692f 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -56,5 +56,11 @@ cmake_args=( "${cmake_toolchain_args[@]}" ) +if [[ "${ROS_VERSION}" == "2" ]]; then + cmake_args+=("-DBUILD_SYSTEM=ROS2") +elif [[ "${ROS_VERSION}" == "1" ]]; then + cmake_args+=("-DBUILD_SYSTEM=ROS1") +fi + CLICOLOR_FORCE=1 NINJA_STATUS="" \ colcon build "${colcon_args[@]}" "$@" --cmake-args "${cmake_args[@]}" diff --git a/.script/rclcpp-log b/.script/rclcpp-log new file mode 100755 index 00000000..b85218f9 --- /dev/null +++ b/.script/rclcpp-log @@ -0,0 +1,67 @@ +#!/bin/bash + +set -euo pipefail + +: "${RMCS_PATH:=$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)}" + +header="${RMCS_PATH}/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp" + +usage() { + cat <<'EOF' +Usage: rclcpp-log + +Commands: + insert Enable RMCS diagnostic RCLCPP logs + cancel Disable RMCS diagnostic RCLCPP logs +EOF +} + +set_state() { + local value="$1" + + if [[ ! -f "${header}" ]]; then + echo "> ERROR: Cannot find ${header}" + exit 1 + fi + + python3 - "$header" "$value" <<'PY' +from pathlib import Path +import re +import sys + +path = Path(sys.argv[1]) +value = sys.argv[2] +text = path.read_text(encoding="utf-8") +updated, count = re.subn( + r"^#define RMCS_RCLCPP_DIAGNOSTIC_LOGS [01]$", + f"#define RMCS_RCLCPP_DIAGNOSTIC_LOGS {value}", + text, + count=1, + flags=re.MULTILINE, +) + +if count != 1: + raise SystemExit(f"> ERROR: Cannot update {path}") + +path.write_text(updated, encoding="utf-8") +PY +} + +case "${1:-}" in +insert) + set_state 1 + echo "> RMCS diagnostic RCLCPP logs enabled. Rebuild rmcs_core to apply." + ;; +cancel) + set_state 0 + echo "> RMCS diagnostic RCLCPP logs disabled. Rebuild rmcs_core to apply." + ;; +-h|--help|help|"") + usage + ;; +*) + echo "> ERROR: Unknown command '${1}'" + usage + exit 1 + ;; +esac diff --git a/Dockerfile b/Dockerfile index ecce1882..fb6b0a34 100644 --- a/Dockerfile +++ b/Dockerfile @@ -30,6 +30,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libceres-dev \ ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-foxglove-bridge \ + ros-$ROS_DISTRO-cv-bridge \ + ros-$ROS_DISTRO-mavros ros-$ROS_DISTRO-mavros-extras \ 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 && \ @@ -37,12 +39,22 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # 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 && \ - 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 && \ +RUN apt-get update && apt-get install -y --no-install-recommends gnupg ca-certificates && \ + mkdir -p /etc/apt/keyrings && \ + wget -qO- https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS.PUB | \ + gpg --dearmor -o /etc/apt/keyrings/intel-openvino.gpg && \ + printf 'Acquire::https::apt.repos.intel.com::Verify-Peer "false";\n' > /etc/apt/apt.conf.d/99intel-openvino-tls && \ + echo "deb [signed-by=/etc/apt/keyrings/intel-openvino.gpg] https://apt.repos.intel.com/openvino ubuntu24 main" > /etc/apt/sources.list.d/intel-openvino.list && \ apt-get update && \ - apt-get install -y --no-install-recommends openvino-2025.2.0 && \ + apt-get install -y --no-install-recommends \ + openvino-2025.2.0 \ + ocl-icd-libopencl1 \ + intel-opencl-icd \ + libze1 \ + libze-intel-gpu1 && \ + rm -f /etc/apt/apt.conf.d/99intel-openvino-tls \ + /etc/apt/sources.list.d/intel-openvino.list \ + /etc/apt/keyrings/intel-openvino.gpg && \ apt-get autoremove -y && apt-get clean && \ rm -rf /var/lib/apt/lists/* /tmp/* @@ -92,6 +104,7 @@ ARG LLVM_VERSION=22 RUN mkdir -p /etc/apt/keyrings && \ wget -qO- https://apt.llvm.org/llvm-snapshot.gpg.key | gpg --dearmor -o /etc/apt/keyrings/apt.llvm.org.gpg && \ chmod 644 /etc/apt/keyrings/apt.llvm.org.gpg && \ + printf 'Acquire::https::apt.llvm.org::Verify-Peer "false";\n' > /etc/apt/apt.conf.d/99llvm-tls && \ echo "deb [signed-by=/etc/apt/keyrings/apt.llvm.org.gpg] https://apt.llvm.org/noble/ llvm-toolchain-noble-${LLVM_VERSION} main" \ > /etc/apt/sources.list.d/llvm.list && \ apt-get update && \ @@ -107,6 +120,7 @@ RUN mkdir -p /etc/apt/keyrings && \ update-alternatives --install /usr/bin/llvm-ar llvm-ar /usr/bin/llvm-ar-${LLVM_VERSION} 50 && \ update-alternatives --install /usr/bin/llvm-ranlib llvm-ranlib /usr/bin/llvm-ranlib-${LLVM_VERSION} 50 && \ update-alternatives --install /usr/bin/ld.lld ld.lld /usr/bin/ld.lld-${LLVM_VERSION} 50 && \ + rm -f /etc/apt/apt.conf.d/99llvm-tls /etc/apt/sources.list.d/llvm.list /etc/apt/keyrings/apt.llvm.org.gpg && \ apt-get autoremove -y && apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* # Generate/load ssh key and setup unison diff --git a/README.md b/README.md index dc365f69..352d85ff 100644 --- a/README.md +++ b/README.md @@ -277,3 +277,5 @@ build-rmcs && wait-sync && attach-remote -r ``` 可以触发 RMCS 构建，`wait-sync` 等待文件同步完成，接下来重启 RMCS 守护进程后，显示实时输出。 + +ps: 无人机自稳需要 `ros-jazzy-mavros` 和 `ros-jazzy-cv-bridge` 这两个包。 diff --git a/rmcs_ws/src/hikcamera b/rmcs_ws/src/hikcamera index 815952fd..f0077f03 160000 --- a/rmcs_ws/src/hikcamera +++ b/rmcs_ws/src/hikcamera @@ -1 +1 @@ -Subproject commit 815952fd1388e121d2014956932f8796a1c8cdd1 +Subproject commit f0077f034800bcd0dde4fffeff270b733772a57e diff --git a/rmcs_ws/src/odin_ros_driver b/rmcs_ws/src/odin_ros_driver new file mode 160000 index 00000000..dbcb0286 --- /dev/null +++ b/rmcs_ws/src/odin_ros_driver @@ -0,0 +1 @@ +Subproject commit dbcb0286b510142e966ac090a37ce5ca238f9e75 diff --git a/rmcs_ws/src/rmcs_auto_aim_v2 b/rmcs_ws/src/rmcs_auto_aim_v2 new file mode 160000 index 00000000..bccd90be --- /dev/null +++ b/rmcs_ws/src/rmcs_auto_aim_v2 @@ -0,0 +1 @@ +Subproject commit bccd90bef9a5edcb21e916e0b56759e1b2638475 diff --git a/rmcs_ws/src/rmcs_bringup/config/flight.yaml b/rmcs_ws/src/rmcs_bringup/config/flight.yaml new file mode 100644 index 00000000..b4ea8830 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/flight.yaml @@ -0,0 +1,233 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::Flight -> flight_hardware + - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware + - rmcs_core::referee::Status -> referee_status + + - rmcs_core::controller::gimbal::HoldController -> hold_controller + - rmcs_core::controller::gimbal::FlightGimbalController -> gimbal_controller + - rmcs::AutoAimComponent -> auto_aim_component + - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_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::referee::command::Interaction -> referee_interaction + #- rmcs_core::referee::command::interaction::Ui -> referee_ui + #- rmcs_core::referee::app::ui::Flight -> referee_ui_flight + + - rmcs_core::referee::Command -> referee_command + + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + - rmcs_core::broadcaster::TfBroadcaster -> tf_broadcaster + +mavros: + ros__parameters: + enabled: false + fcu_url: serial:///dev/ttyACM0:921600 + gcs_url: "" + target_system_id: 1 + target_component_id: 1 + fcu_protocol: v2.0 + respawn: true + respawn_delay: 1.0 + +odin_ros_driver: + ros__parameters: + # FlightMavros reads odom -> odin1_base_link TF and forwards it to MAVROS vision pose. + enabled: true + config_file: "/rmcs_install/share/odin_ros_driver/config/control_command.yaml" # Empty uses odin_ros_driver/config/control_command.yaml + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 + +auto_aim_runtime: + ros__parameters: + enabled: true + package: rmcs_auto_aim_v2 + executable: rmcs_auto_aim_v2_runtime + node_name: auto_aim_runtime + respawn: true + respawn_delay: 1.0 + +value_broadcaster: + ros__parameters: + forward_list: + - /gimbal/pitch/angle + - /gimbal/pitch/velocity + - /gimbal/pitch/torque + # - /gimbal/pitch/control_torque + - /gimbal/pitch/control_angle_error + - /gimbal/pitch/control_velocity + - /gimbal/pitch/velocity_imu + + - /gimbal/yaw/angle + - /gimbal/yaw/velocity + - /gimbal/yaw/torque + - /gimbal/yaw/control_torque + - /gimbal/yaw/control_angle_error + - /gimbal/yaw/control_velocity + +tf_broadcaster: + ros__parameters: + tf: /tf + +flight_hardware: + ros__parameters: + board_serial: "d4-9d44" + yaw_motor_zero_point: 38745 + pitch_motor_zero_point: 51586 + +flight_mavros_hardware: + ros__parameters: + target_frame_id: odom + source_frame_id: odin1_base_link + output_rate: 30.0 + sensor_roll_offset_rad: 3.141592653589793 + sensor_pitch_offset_rad: 1.5707963267948966 + sensor_yaw_offset_rad: 0.0 + mavros_pose_topic: /mavros/vision_pose/pose + mavros_pose_frame_id: odom + +referee_status: + ros__parameters: + path: /dev/tty0 + +hold_controller: + ros__parameters: + joystick_deadband: 0.03 + mouse_deadband: 0.5 + +gimbal_controller: + ros__parameters: + upper_limit: -0.15 + lower_limit: 0.50 + yaw_upper_limit: -0.349 + yaw_lower_limit: 3.14 + hold_level_threshold: 0.01 + hold_yaw_kd: 8.0 + hold_pitch_kd: 0.5 + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + feedforward: /gimbal/yaw/hold_feedforward + control: /gimbal/yaw/control_torque + kp: 70.8943 + ki: 0.0 + kd: 1440.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + feedforward: /gimbal/pitch/hold_feedforward + control: /gimbal/pitch/control_velocity + kp: 10.0 + ki: 0.0 + kd: 400.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 740.0 + - 740.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 8.0 + shot_frequency: 20.0 + safe_shot_frequency: 10.0 + eject_frequency: 10.0 + eject_time: 0.05 + deep_eject_frequency: 5.0 + deep_eject_time: 0.2 + single_shot_max_stop_delay: 2.0 + +shooting_recorder: + ros__parameters: + friction_wheel_count: 2 + # 1: trigger, 2: timing + log_mode: 1 + +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.583 + ki: 0.0 + kd: 0.0 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 3 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: 0.02 + pitch_error: -0.01 + shoot_velocity: 28.0 + predict_sec: 0.095 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: true + raw_img_pub: false # Set false in actual use + image_viewer_type: 2 diff --git a/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml b/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml new file mode 100644 index 00000000..1c6d8e22 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/mavros_bridge.yaml @@ -0,0 +1,36 @@ +--- +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::FlightMavros -> flight_mavros_hardware +mavros: + ros__parameters: + enabled: true + fcu_url: serial:///dev/ttyACM0:921600 + gcs_url: "" + target_system_id: 1 + target_component_id: 1 + fcu_protocol: v2.0 + respawn: true + respawn_delay: 1.0 +odin_ros_driver: + ros__parameters: + enabled: true + config_file: >- + /rmcs_install/share/odin_ros_driver/config/control_command.yaml + node_name: host_sdk_sample + respawn: true + respawn_delay: 1.0 +flight_mavros_hardware: + ros__parameters: + target_frame_id: odom + source_frame_id: odin1_base_link + output_rate: 30.0 + # Keep odom position as-is; only correct the vision sensor orientation for + # the reversed front-facing Odin mount. + sensor_roll_offset_rad: 3.141592653589793 + sensor_pitch_offset_rad: 1.5707963267948966 + sensor_yaw_offset_rad: 0.0 + mavros_pose_topic: /mavros/vision_pose/pose + mavros_pose_frame_id: odom diff --git a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py index 976cb9e7..c53faec4 100644 --- a/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py +++ b/rmcs_ws/src/rmcs_bringup/launch/rmcs.launch.py @@ -1,5 +1,6 @@ from typing import List, Optional import os +import yaml from launch import ( LaunchContext, @@ -46,10 +47,133 @@ def visit( ], respawn=True, respawn_delay=1.0, - output="log", # stdout and stderr are logged to launch log file and stderr to the screen. + output="screen", + emulate_tty=True, ) ) + # Conditionally launch optional processes declared in the config yaml. + config_path = os.path.join( + FindPackageShare("rmcs_bringup").perform(context), + "config", + robot_name + ".yaml", + ) + with open(config_path, "r") as f: + config = yaml.safe_load(f) + + odin_cfg = config.get("odin_ros_driver", {}).get("ros__parameters") + if odin_cfg is not None and odin_cfg.get("enabled", True): + odin_share = FindPackageShare("odin_ros_driver").perform(context) + default_odin_config_file = os.path.join( + odin_share, "config", "control_command.yaml" + ) + odin_config_file = odin_cfg.get("config_file") or default_odin_config_file + if not os.path.isfile(odin_config_file): + entities.append( + LogInfo( + msg=( + f"Odin config '{odin_config_file}' was not found; " + f"falling back to '{default_odin_config_file}'" + ) + ) + ) + odin_config_file = default_odin_config_file + node_name = odin_cfg.get("node_name", "host_sdk_sample") + respawn = odin_cfg.get("respawn", True) + respawn_delay = float(odin_cfg.get("respawn_delay", 1.0)) + entities.append( + LogInfo( + msg=( + f"Starting odin_ros_driver node '{node_name}' " + f"with config '{odin_config_file}'" + ) + ) + ) + entities.append( + Node( + package="odin_ros_driver", + executable="host_sdk_sample", + name=node_name, + parameters=[{"config_file": odin_config_file}], + respawn=respawn, + respawn_delay=respawn_delay, + output="screen", + emulate_tty=True, + ) + ) + + auto_aim_cfg = config.get("auto_aim_runtime", {}).get("ros__parameters") + if auto_aim_cfg is not None and auto_aim_cfg.get("enabled", True): + package = auto_aim_cfg.get("package", "rmcs_auto_aim_v2") + executable = auto_aim_cfg.get("executable", "rmcs_auto_aim_v2_runtime") + node_name = auto_aim_cfg.get("node_name", "auto_aim_runtime") + respawn = auto_aim_cfg.get("respawn", True) + respawn_delay = float(auto_aim_cfg.get("respawn_delay", 1.0)) + entities.append( + LogInfo( + msg=f"Starting auto aim runtime '{node_name}' from {package}/{executable}" + ) + ) + entities.append( + Node( + package=package, + executable=executable, + name=node_name, + respawn=respawn, + respawn_delay=respawn_delay, + output="screen", + emulate_tty=True, + ) + ) + + mavros_cfg = config.get("mavros", {}).get("ros__parameters") + if mavros_cfg is not None and mavros_cfg.get("enabled", True): + mavros_share = FindPackageShare("mavros").perform(context) + pluginlists_yaml = os.path.join( + mavros_share, "launch", "px4_pluginlists.yaml" + ) + px4_config_yaml = os.path.join(mavros_share, "launch", "px4_config.yaml") + fcu_url = mavros_cfg.get( + "fcu_url", + f"serial://{mavros_cfg.get('device', '/dev/ttyACM0')}:{mavros_cfg.get('baudrate', 921600)}", + ) + gcs_url = mavros_cfg.get("gcs_url", "") + target_system_id = int(mavros_cfg.get("target_system_id", 1)) + target_component_id = int(mavros_cfg.get("target_component_id", 1)) + fcu_protocol = mavros_cfg.get("fcu_protocol", "v2.0") + respawn = mavros_cfg.get("respawn", True) + respawn_delay = float(mavros_cfg.get("respawn_delay", 1.0)) + entities.append( + LogInfo( + msg=( + f"Starting ROS 2 MAVROS on '{fcu_url}' " + f"(target {target_system_id}:{target_component_id})" + ) + ) + ) + entities.append( + Node( + package="mavros", + executable="mavros_node", + namespace="mavros", + parameters=[ + pluginlists_yaml, + px4_config_yaml, + { + "fcu_url": fcu_url, + "gcs_url": gcs_url, + "tgt_system": target_system_id, + "tgt_component": target_component_id, + "fcu_protocol": fcu_protocol, + }, + ], + respawn=respawn, + respawn_delay=respawn_delay, + output="screen", + emulate_tty=True, + ) + ) + if is_automatic: pass diff --git a/rmcs_ws/src/rmcs_bringup/package.xml b/rmcs_ws/src/rmcs_bringup/package.xml index c64c661f..38a8170a 100644 --- a/rmcs_ws/src/rmcs_bringup/package.xml +++ b/rmcs_ws/src/rmcs_bringup/package.xml @@ -11,6 +11,9 @@ ament_cmake joint_state_broadcaster + mavros + odin_ros_driver + rmcs_auto_aim_v2 robot_state_publisher rviz2 xacro @@ -21,4 +24,4 @@ ament_cmake - \ 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 dd3c8579..529ee90a 100644 --- a/rmcs_ws/src/rmcs_core/CMakeLists.txt +++ b/rmcs_ws/src/rmcs_core/CMakeLists.txt @@ -17,8 +17,8 @@ include(FetchContent) set(BUILD_STATIC_LIBRMCS ON CACHE BOOL "Build static librmcs SDK" FORCE) FetchContent_Declare( librmcs - URL https://github.com/Alliance-Algorithm/librmcs/releases/download/v3.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) @@ -35,7 +35,14 @@ ament_auto_add_library( include_directories(${PROJECT_SOURCE_DIR}/include) include_directories(${PROJECT_SOURCE_DIR}/src) -target_link_libraries(${PROJECT_NAME} librmcs-sdk) + +target_link_libraries( + ${PROJECT_NAME} + + librmcs-sdk + pthread + ${CMAKE_DL_LIBS} +) pluginlib_export_plugin_description_file(rmcs_executor plugins.xml) diff --git a/rmcs_ws/src/rmcs_core/package.xml b/rmcs_ws/src/rmcs_core/package.xml index 41074552..a49a7a89 100644 --- a/rmcs_ws/src/rmcs_core/package.xml +++ b/rmcs_ws/src/rmcs_core/package.xml @@ -9,17 +9,19 @@ ament_cmake + nav_msgs + geometry_msgs rclcpp std_msgs pluginlib tf2 + tf2_msgs tf2_ros serial rmcs_utility rmcs_msgs rmcs_executor rmcs_description - 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 f7847151..d8db579a 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -1,4 +1,6 @@ + + @@ -8,6 +10,8 @@ + + diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp new file mode 100644 index 00000000..23ff90cb --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/flight_gimbal_controller.cpp @@ -0,0 +1,394 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +using namespace rmcs_description; // NOLINT(google-build-using-namespace) + +class FlightTwoAxisGimbalSolver { + class Operation { + friend class FlightTwoAxisGimbalSolver; + + virtual PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const = 0; + }; + +public: + FlightTwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) + : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { + component.register_input("/tf", tf_); + } + + // Re-anchor the stored control direction to the gimbal's current physical pose. + // Called on hold-mode exit so that the next SetControlShift starts from "here", + // not from a stale target that predates the hold period. + void anchor_to_current_pose() { + control_direction_ = + fast_tf::cast(PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *tf_); + control_enabled_ = true; + } + + class SetDisabled : public Operation { + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + solver.control_enabled_ = false; + return {}; + } + }; + + class SetToLevel : public Operation { + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + auto odom_direction = fast_tf::cast( + PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}, *solver.tf_); + if (std::abs(odom_direction->x()) < 1e-6 && std::abs(odom_direction->y()) < 1e-6) + return {}; + + solver.control_enabled_ = true; + odom_direction->z() = 0; + auto pitch_direction = fast_tf::cast(odom_direction, *solver.tf_); + pitch_direction->normalize(); + return pitch_direction; + } + }; + + class SetControlDirection : public Operation { + public: + explicit SetControlDirection(OdomImu::DirectionVector target) + : target_(std::move(target)) {} + + private: + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + solver.control_enabled_ = true; + return fast_tf::cast(target_, *solver.tf_); + } + + OdomImu::DirectionVector target_; + }; + + class SetControlShift : public Operation { + public: + SetControlShift(double yaw_shift, double pitch_shift) + : yaw_shift_(yaw_shift) + , pitch_shift_(pitch_shift) {} + + private: + PitchLink::DirectionVector update(FlightTwoAxisGimbalSolver& solver) const override { + PitchLink::DirectionVector direction; + if (!solver.control_enabled_) { + solver.control_enabled_ = true; + direction = PitchLink::DirectionVector{Eigen::Vector3d::UnitX()}; + } else { + direction = fast_tf::cast(solver.control_direction_, *solver.tf_); + } + + auto yaw_transform = Eigen::AngleAxisd{yaw_shift_, Eigen::Vector3d::UnitZ()}; + auto pitch_transform = Eigen::AngleAxisd{pitch_shift_, Eigen::Vector3d::UnitY()}; + return PitchLink::DirectionVector{pitch_transform * (yaw_transform * (*direction))}; + } + + double yaw_shift_; + double pitch_shift_; + }; + + struct AngleError { + double yaw_angle_error; + double pitch_angle_error; + }; + + AngleError update(const Operation& operation) { + update_yaw_axis(); + + auto control_direction = operation.update(*this); + if (!control_enabled_) + return {kNan, kNan}; + + auto [control_direction_yaw_link, pitch] = pitch_link_to_yaw_link(control_direction); + clamp_control_direction(control_direction_yaw_link); + if (!control_enabled_) + return {kNan, kNan}; + + control_direction_ = + fast_tf::cast(yaw_link_to_pitch_link(control_direction_yaw_link, pitch), *tf_); + return calculate_control_errors(control_direction_yaw_link, pitch); + } + + bool enabled() const { return control_enabled_; } + +private: + void update_yaw_axis() { + auto yaw_axis = + fast_tf::cast(YawLink::DirectionVector{Eigen::Vector3d::UnitZ()}, *tf_); + *yaw_axis_filtered_ += 0.1 * (*fast_tf::cast(yaw_axis, *tf_)); + yaw_axis_filtered_->normalize(); + } + + auto pitch_link_to_yaw_link(const PitchLink::DirectionVector& direction) const + -> std::pair { + std::pair result; + auto& [direction_yaw_link, pitch] = result; + + auto yaw_axis = fast_tf::cast(yaw_axis_filtered_, *tf_); + pitch = {yaw_axis->z(), yaw_axis->x()}; + pitch.normalize(); + + const auto& [x, y, z] = *direction; + direction_yaw_link = {x * pitch.x() - z * pitch.y(), y, x * pitch.y() + z * pitch.x()}; + return result; + } + + static PitchLink::DirectionVector yaw_link_to_pitch_link( + const YawLink::DirectionVector& direction, const Eigen::Vector2d& pitch) { + const auto& [x, y, z] = *direction; + return {x * pitch.x() + z * pitch.y(), y, -x * pitch.y() + z * pitch.x()}; + } + + void clamp_control_direction(YawLink::DirectionVector& control_direction) { + const auto& [x, y, z] = *control_direction; + + Eigen::Vector2d xy_projection{x, y}; + double xy_norm = xy_projection.norm(); + if (xy_norm <= 0.0) { + control_enabled_ = false; + return; + } + xy_projection /= xy_norm; + + if (z > upper_limit_.y()) { + *control_direction << upper_limit_.x() * xy_projection, upper_limit_.y(); + } else if (z < lower_limit_.y()) { + *control_direction << lower_limit_.x() * xy_projection, lower_limit_.y(); + } + + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm <= 0.0) { + control_enabled_ = false; + return; + } + xz_projection /= xz_norm; + + if (yaw_y > yaw_upper_limit_.y()) { + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), yaw_upper_limit_.y(), + yaw_upper_limit_.x() * xz_projection.y(); + } else if (yaw_y < yaw_lower_limit_.y()) { + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), yaw_lower_limit_.y(), + yaw_lower_limit_.x() * xz_projection.y(); + } + } + + static AngleError calculate_control_errors( + const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { + const auto& [x, y, z] = *control_direction; + const auto& [c, s] = pitch; + + AngleError result; + result.yaw_angle_error = std::atan2(y, x); + double x_projected = std::sqrt(x * x + y * y); + result.pitch_angle_error = -std::atan2(z * c - x_projected * s, z * s + x_projected * c); + return result; + } + + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + + const Eigen::Vector2d upper_limit_; + const Eigen::Vector2d lower_limit_; + const Eigen::Vector2d yaw_upper_limit_; + const Eigen::Vector2d yaw_lower_limit_; + + rmcs_executor::Component::InputInterface tf_; + + OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; + bool control_enabled_ = false; + OdomImu::DirectionVector control_direction_; +}; + +class FlightGimbalController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FlightGimbalController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , hold_level_threshold_(get_parameter("hold_level_threshold").as_double()) + , hold_yaw_kd_(get_parameter("hold_yaw_kd").as_double()) + , hold_pitch_kd_(get_parameter("hold_pitch_kd").as_double()) + , solver_( + *this, get_parameter("upper_limit").as_double(), + get_parameter("lower_limit").as_double(), + get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()) { + 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("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); + register_input("/gimbal/yaw/velocity_imu", yaw_velocity_imu_); + register_input("/gimbal/pitch/velocity_imu", pitch_velocity_imu_); + register_input("/gimbal/yaw/velocity", yaw_velocity_encoder_); + register_input("/gimbal/pitch/velocity", pitch_velocity_encoder_); + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + + // Optional: when HoldController is not loaded these stay unbound, + // calculate_angle_error() falls through to the pre-hold behavior. + register_input("/gimbal/hold/desired", hold_desired_, false); + + register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, kNan); + register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, kNan); + register_output("/gimbal/yaw/hold_feedforward", yaw_hold_feedforward_, 0.0); + register_output("/gimbal/pitch/hold_feedforward", pitch_hold_feedforward_, 0.0); + } + + void update() override { + auto angle_error = calculate_angle_error(); + *yaw_angle_error_ = angle_error.yaw_angle_error; + + if (!std::isfinite(angle_error.pitch_angle_error) || hold_active_) { + *pitch_angle_error_ = angle_error.pitch_angle_error; + } else if (std::abs(angle_error.pitch_angle_error) < kPitchDeadband) { + *pitch_angle_error_ = 0.0; + } else if (angle_error.pitch_angle_error > 0.0) { + *pitch_angle_error_ = angle_error.pitch_angle_error - kPitchDeadband; + } else { + *pitch_angle_error_ = angle_error.pitch_angle_error + kPitchDeadband; + } + + if (hold_active_) { + filtered_yaw_vel_ += kVelFilterAlpha * (*yaw_velocity_imu_ - filtered_yaw_vel_); + filtered_pitch_vel_ += kVelFilterAlpha * (*pitch_velocity_imu_ - filtered_pitch_vel_); + *yaw_hold_feedforward_ = -hold_yaw_kd_ * filtered_yaw_vel_; + *pitch_hold_feedforward_ = -hold_pitch_kd_ * filtered_pitch_vel_; + } else { + filtered_yaw_vel_ = 0.0; + filtered_pitch_vel_ = 0.0; + *yaw_hold_feedforward_ = 0.0; + *pitch_hold_feedforward_ = 0.0; + } + } + +private: + auto calculate_angle_error() -> FlightTwoAxisGimbalSolver::AngleError { + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + + using namespace rmcs_msgs; // NOLINT(google-build-using-namespace) + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + level_reached_ = false; + hold_active_ = false; + return solver_.update(FlightTwoAxisGimbalSolver::SetDisabled{}); + } + + if (!solver_.enabled()) { + level_reached_ = false; + hold_active_ = false; + return solver_.update(FlightTwoAxisGimbalSolver::SetToLevel{}); + } + + // Until SetToLevel has converged, hold/auto-aim mode is forbidden so an + // idle input at startup cannot latch a target before leveling finishes. + if (!level_reached_) { + auto err = solver_.update(FlightTwoAxisGimbalSolver::SetToLevel{}); + if (std::isfinite(err.yaw_angle_error) && std::isfinite(err.pitch_angle_error) + && std::abs(err.yaw_angle_error) < hold_level_threshold_ + && std::abs(err.pitch_angle_error) < hold_level_threshold_) { + level_reached_ = true; + } + return err; + } + + if (auto_aim_control_direction_.ready() && (mouse_->right || switch_right == Switch::UP) + && !auto_aim_control_direction_->isZero()) { + hold_active_ = false; + return solver_.update( + FlightTwoAxisGimbalSolver::SetControlDirection( + OdomImu::DirectionVector{*auto_aim_control_direction_})); + } + + const bool want_hold = hold_desired_.ready() && *hold_desired_; + if (want_hold) { + if (!hold_active_) { + hold_active_ = true; + hold_target_yaw_ = *gimbal_yaw_angle_; + hold_target_pitch_ = *gimbal_pitch_angle_; + } + return { + wrap_pi(hold_target_yaw_ - *gimbal_yaw_angle_), + wrap_pi(hold_target_pitch_ - *gimbal_pitch_angle_)}; + } + + if (hold_active_) { + hold_active_ = false; + solver_.anchor_to_current_pose(); + } + + constexpr double joystick_sensitivity = 0.006; + constexpr double mouse_sensitivity = 0.5; + double yaw_shift = + joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); + double pitch_shift = + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); + + return solver_.update(FlightTwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); + } + + static double wrap_pi(double x) { return std::remainder(x, 2.0 * std::numbers::pi); } + + static constexpr double kNan = std::numeric_limits::quiet_NaN(); + static constexpr double kPitchDeadband = 2e-3; + static constexpr double kVelFilterAlpha = 0.12; + + const double hold_level_threshold_; + const double hold_yaw_kd_; + const double hold_pitch_kd_; + bool level_reached_ = false; + bool hold_active_ = false; + double hold_target_yaw_ = 0.0; + double hold_target_pitch_ = 0.0; + double filtered_yaw_vel_ = 0.0; + double filtered_pitch_vel_ = 0.0; + + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface auto_aim_control_direction_; + InputInterface yaw_velocity_imu_; + InputInterface pitch_velocity_imu_; + InputInterface yaw_velocity_encoder_; + InputInterface pitch_velocity_encoder_; + InputInterface gimbal_yaw_angle_; + InputInterface gimbal_pitch_angle_; + InputInterface hold_desired_; + + OutputInterface yaw_angle_error_; + OutputInterface pitch_angle_error_; + OutputInterface yaw_hold_feedforward_; + OutputInterface pitch_hold_feedforward_; + + FlightTwoAxisGimbalSolver solver_; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::gimbal::FlightGimbalController, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp index f20bd98f..284af8a6 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hero_gimbal_controller.cpp @@ -1,5 +1,3 @@ -#include - #include #include #include @@ -8,9 +6,6 @@ #include #include -#include "controller/gimbal/precise_two_axis_gimbal_solver.hpp" -#include "controller/gimbal/two_axis_gimbal_solver.hpp" - namespace rmcs_core::controller::gimbal { using namespace rmcs_description; @@ -22,150 +17,9 @@ class HeroGimbalController HeroGimbalController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , imu_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) - , encoder_gimbal_solver( - *this, get_parameter("upper_limit").as_double(), - get_parameter("lower_limit").as_double()) { - - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); - register_input("/remote/keyboard", keyboard_); - - register_input("/gimbal/auto_aim/control_direction", auto_aim_control_direction_, false); - - register_output("/gimbal/mode", gimbal_mode_, rmcs_msgs::GimbalMode::IMU); - - register_output("/gimbal/yaw/control_angle_error", yaw_angle_error_, nan_); - register_output("/gimbal/pitch/control_angle_error", pitch_angle_error_, nan_); - register_output("/gimbal/yaw/control_angle_shift", yaw_control_angle_shift_, nan_); - register_output("/gimbal/pitch/control_angle", pitch_control_angle_, nan_); - } - - void update() override { - const auto& switch_left = *switch_left_; - const auto& switch_right = *switch_right_; - - do { - using namespace rmcs_msgs; - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_control(); - break; - } - - if (!last_keyboard_.q && keyboard_->q) { - if (gimbal_mode_keyboard_ == GimbalMode::IMU) - gimbal_mode_keyboard_ = GimbalMode::ENCODER; - else - gimbal_mode_keyboard_ = GimbalMode::IMU; - } - *gimbal_mode_ = - *switch_right_ == Switch::UP ? GimbalMode::ENCODER : gimbal_mode_keyboard_; - - if (*gimbal_mode_ == GimbalMode::IMU) { - auto angle_error = update_imu_control(); - *yaw_angle_error_ = angle_error.yaw_angle_error; - *pitch_angle_error_ = angle_error.pitch_angle_error; - - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); - *yaw_control_angle_shift_ = nan_; - *pitch_control_angle_ = nan_; - } else { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - *yaw_angle_error_ = nan_; - *pitch_angle_error_ = nan_; - - auto control_angle = update_encoder_control(); - *yaw_control_angle_shift_ = control_angle.yaw_shift; - *pitch_control_angle_ = control_angle.pitch_angle; - } - } while (false); - - last_keyboard_ = *keyboard_; - } - - void reset_all_control() { - imu_gimbal_solver.update(TwoAxisGimbalSolver::SetDisabled{}); - encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetDisabled{}); - - gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; - *gimbal_mode_ = rmcs_msgs::GimbalMode::IMU; - - *yaw_angle_error_ = nan_; - *pitch_angle_error_ = nan_; - *yaw_control_angle_shift_ = nan_; - *pitch_control_angle_ = nan_; - } - - TwoAxisGimbalSolver::AngleError update_imu_control() { - if (auto_aim_control_direction_.ready() - && (mouse_->right || *switch_right_ == rmcs_msgs::Switch::UP) - && !auto_aim_control_direction_->isZero()) { - return imu_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlDirection{ - OdomImu::DirectionVector{*auto_aim_control_direction_}}); - } - - if (!imu_gimbal_solver.enabled()) - return imu_gimbal_solver.update(TwoAxisGimbalSolver::SetToLevel{}); - - constexpr double joystick_sensitivity = 0.006; - constexpr double mouse_sensitivity = 0.5; - - double yaw_shift = - joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); - double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); - - return imu_gimbal_solver.update( - TwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); - } - - PreciseTwoAxisGimbalSolver::ControlAngle update_encoder_control() { - if (!encoder_gimbal_solver.enabled()) - return encoder_gimbal_solver.update(PreciseTwoAxisGimbalSolver::SetControlPitch{-0.31}); - - constexpr double joystick_sensitivity = 0.006 * 0.1; - constexpr double mouse_yaw_sensitivity = 0.5 * 0.114; - constexpr double mouse_pitch_sensitivity = 0.5 * 0.095; - - double yaw_shift = joystick_sensitivity * joystick_left_->y() - + mouse_yaw_sensitivity * mouse_velocity_->y(); - double pitch_shift = -joystick_sensitivity * joystick_left_->x() - - mouse_pitch_sensitivity * mouse_velocity_->x(); - - return encoder_gimbal_solver.update( - PreciseTwoAxisGimbalSolver::SetControlShift{yaw_shift, pitch_shift}); - } - -private: - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); - - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; - InputInterface keyboard_; - - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); - - InputInterface auto_aim_control_direction_; - - rmcs_msgs::GimbalMode gimbal_mode_keyboard_ = rmcs_msgs::GimbalMode::IMU; - OutputInterface gimbal_mode_; - - TwoAxisGimbalSolver imu_gimbal_solver; - PreciseTwoAxisGimbalSolver encoder_gimbal_solver; + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {} - OutputInterface yaw_angle_error_, pitch_angle_error_; - OutputInterface yaw_control_angle_shift_, pitch_control_angle_; + void update() override {} }; } // namespace rmcs_core::controller::gimbal diff --git a/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp new file mode 100644 index 00000000..ce7bfad9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/gimbal/hold_controller.cpp @@ -0,0 +1,74 @@ +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::gimbal { + +class HoldController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + HoldController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , joystick_deadband_(get_parameter("joystick_deadband").as_double()) + , mouse_deadband_(get_parameter("mouse_deadband").as_double()) { + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/switch/right", switch_right_); + + register_output("/gimbal/hold/desired", hold_desired_, false); + } + + void update() override { + using namespace rmcs_msgs; // NOLINT(google-build-using-namespace) + const auto switch_left = *switch_left_; + const auto switch_right = *switch_right_; + + const bool gimbal_disabled = switch_left == Switch::UNKNOWN + || switch_right == Switch::UNKNOWN + || (switch_left == Switch::DOWN && switch_right == Switch::DOWN); + + if (gimbal_disabled || !is_idle()) { + idle_counter_ = 0; + *hold_desired_ = false; + return; + } + + idle_counter_ = std::min(idle_counter_ + 1, kEntryTicks); + *hold_desired_ = idle_counter_ >= kEntryTicks; + } + +private: + [[nodiscard]] bool is_idle() const { + const auto joystick_norm = joystick_left_->norm(); + const auto mouse_norm = mouse_velocity_->norm(); + return std::isfinite(joystick_norm) && std::isfinite(mouse_norm) + && joystick_norm < joystick_deadband_ && mouse_norm < mouse_deadband_; + } + + static constexpr int kEntryTicks = 50; + + const double joystick_deadband_; + const double mouse_deadband_; + int idle_counter_ = 0; + + InputInterface joystick_left_; + InputInterface mouse_velocity_; + InputInterface switch_left_; + InputInterface switch_right_; + + OutputInterface hold_desired_; +}; + +} // namespace rmcs_core::controller::gimbal + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::gimbal::HoldController, 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..06081af2 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,5 +1,8 @@ +#include #include +#include +#include #include #include #include @@ -7,6 +10,7 @@ #include #include "controller/gimbal/two_axis_gimbal_solver.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::gimbal { @@ -22,7 +26,9 @@ class SimpleGimbalController 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()) { + get_parameter("lower_limit").as_double(), + get_parameter("yaw_upper_limit").as_double(), + get_parameter("yaw_lower_limit").as_double()) { register_input("/remote/joystick/left", joystick_left_); register_input("/remote/switch/right", switch_right_); @@ -39,7 +45,18 @@ class SimpleGimbalController void update() override { auto angle_error = calculate_angle_error(); *yaw_angle_error_ = angle_error.yaw_angle_error; - *pitch_angle_error_ = angle_error.pitch_angle_error; + if (abs(angle_error.pitch_angle_error) < 8e-3) { + *pitch_angle_error_ = 0; + } else { + *pitch_angle_error_ = angle_error.pitch_angle_error; + } + + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Gimbal controller output: yaw_error=%.6f pitch_error=%.6f", + *yaw_angle_error_, *pitch_angle_error_); + } } TwoAxisGimbalSolver::AngleError calculate_angle_error() { @@ -47,6 +64,17 @@ class SimpleGimbalController auto switch_left = *switch_left_; auto mouse = *mouse_; + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Gimbal remote input: switch_left=%u switch_right=%u joystick_left=(%.3f, %.3f) " + "mouse_velocity=(%.3f, %.3f) mouse_left=%d mouse_right=%d", + static_cast(std::to_underlying(switch_left)), + static_cast(std::to_underlying(switch_right)), joystick_left_->x(), + joystick_left_->y(), mouse_velocity_->x(), mouse_velocity_->y(), mouse.left, + mouse.right); + } + using namespace rmcs_msgs; if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) @@ -67,7 +95,10 @@ class SimpleGimbalController double yaw_shift = joystick_sensitivity * joystick_left_->y() + mouse_sensitivity * mouse_velocity_->y(); double pitch_shift = - -joystick_sensitivity * joystick_left_->x() - mouse_sensitivity * mouse_velocity_->x(); + -joystick_sensitivity * joystick_left_->x() + mouse_sensitivity * mouse_velocity_->x(); + + // RCLCPP_INFO(get_logger(), "[gimbal calibration] New pitch offset: %f", + // joystick_left_->x()); return two_axis_gimbal_solver.update( TwoAxisGimbalSolver::SetControlShift(yaw_shift, pitch_shift)); @@ -94,4 +125,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..f163e656 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 @@ -1,14 +1,12 @@ #pragma once +#include #include -#include #include #include #include -#include -#include #include #include #include @@ -26,11 +24,16 @@ class TwoAxisGimbalSolver { }; public: - TwoAxisGimbalSolver(rmcs_executor::Component& component, double upper_limit, double lower_limit) + TwoAxisGimbalSolver( + rmcs_executor::Component& component, double upper_limit, double lower_limit, + double yaw_upper_limit, double yaw_lower_limit) : upper_limit_(std::cos(upper_limit), -std::sin(upper_limit)) - , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) { + , lower_limit_(std::cos(lower_limit), -std::sin(lower_limit)) + , yaw_upper_limit_(std::cos(yaw_upper_limit), -std::sin(yaw_upper_limit)) + , yaw_lower_limit_(std::cos(yaw_lower_limit), -std::sin(yaw_lower_limit)) { component.register_input("/gimbal/pitch/angle", gimbal_pitch_angle_); + component.register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); component.register_input("/tf", tf_); } @@ -45,7 +48,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 (odom_dir->x() == 0 && odom_dir->y() == 0) return {}; super.control_enabled_ = true; @@ -104,6 +107,7 @@ class TwoAxisGimbalSolver { update_yaw_axis(); PitchLink::DirectionVector control_direction = operation.update(*this); + if (!control_enabled_) return {nan_, nan_}; @@ -136,7 +140,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()}; @@ -167,8 +171,24 @@ class TwoAxisGimbalSolver { *control_direction << upper_limit_.x() * projection, upper_limit_.y(); else if (z < lower_limit_.y()) *control_direction << lower_limit_.x() * projection, lower_limit_.y(); + + const auto& [yaw_x, yaw_y, yaw_z] = *control_direction; + + Eigen::Vector2d xz_projection{yaw_x, yaw_z}; + double xz_norm = xz_projection.norm(); + if (xz_norm > 0) + xz_projection /= xz_norm; + + if (yaw_y > yaw_upper_limit_.y()) + *control_direction << yaw_upper_limit_.x() * xz_projection.x(), + yaw_upper_limit_.y(), yaw_upper_limit_.x() * xz_projection.y(); + else if (yaw_y < yaw_lower_limit_.y()) + *control_direction << yaw_lower_limit_.x() * xz_projection.x(), + yaw_lower_limit_.y(), yaw_lower_limit_.x() * xz_projection.y(); + } + static AngleError calculate_control_errors( const YawLink::DirectionVector& control_direction, const Eigen::Vector2d& pitch) { const auto& [x, y, z] = *control_direction; @@ -185,8 +205,10 @@ class TwoAxisGimbalSolver { static constexpr double nan_ = std::numeric_limits::quiet_NaN(); const Eigen::Vector2d upper_limit_, lower_limit_; + const Eigen::Vector2d yaw_upper_limit_, yaw_lower_limit_; rmcs_executor::Component::InputInterface gimbal_pitch_angle_; + rmcs_executor::Component::InputInterface gimbal_yaw_angle_; rmcs_executor::Component::InputInterface tf_; OdomImu::DirectionVector yaw_axis_filtered_{Eigen::Vector3d::UnitZ()}; @@ -195,4 +217,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/pid/error_pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp index ac334dab..37eb03e3 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp @@ -5,6 +5,7 @@ #include "controller/pid/pid_calculator.hpp" #include "controller/pid/smart_input.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::pid { @@ -37,6 +38,12 @@ class ErrorPidController void update() override { auto err = *measurement_; *control_ = *feedforward_ + pid_calculator_.update(err); + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Error PID output: measurement=%.6f feedforward=%.6f control=%.6f", err, + *feedforward_, *control_); + } } private: @@ -51,4 +58,4 @@ class ErrorPidController #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) 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 9780edd3..239253b0 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 @@ -5,6 +5,7 @@ #include "controller/pid/pid_calculator.hpp" #include "controller/pid/smart_input.hpp" +#include "debug/rclcpp_diagnostic_log.hpp" namespace rmcs_core::controller::pid { @@ -38,6 +39,12 @@ class PidController void update() override { auto err = *setpoint_ - *measurement_; *control_ = *feedforward_ + pid_calculator_.update(err); + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "PID output: measurement=%.6f setpoint=%.6f feedforward=%.6f control=%.6f", + *measurement_, *setpoint_, *feedforward_, *control_); + } } private: diff --git a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp index 3de0960a..3fa01d55 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/shooting/heat_controller.cpp @@ -32,7 +32,7 @@ class HeatController shooter_heat_ += heat_per_shot + 10; *control_bullet_allowance_ = std::max( - 0, (*shooter_heat_limit_ - shooter_heat_ - reserved_heat) / heat_per_shot); + 0, (100 - shooter_heat_ - reserved_heat) / heat_per_shot); } private: diff --git a/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp b/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp new file mode 100644 index 00000000..55949da3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/debug/rclcpp_diagnostic_log.hpp @@ -0,0 +1,3 @@ +#pragma once + +#define RMCS_RCLCPP_DIAGNOSTIC_LOGS 0 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp index 75152b17..3ba98025 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dr16.hpp @@ -81,8 +81,12 @@ class Dr16 { auto channel_to_double = [](int32_t value) { value -= 1024; - if (-660 <= value && value <= 660) + constexpr int32_t deadzone = 10; + if (-660 <= value && value <= 660) { + if (value > -deadzone && value < deadzone) + return 0.0; return value / 660.0; + } return 0.0; }; joystick_right_.y = -channel_to_double(static_cast(part1.joystick_channel0)); 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..67d28883 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 @@ -23,7 +23,7 @@ namespace rmcs_core::hardware::device { class LkMotor { public: - enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10 }; + enum class Type : uint8_t { kMG5010Ei10, kMG4010Ei10, kMG6012Ei8, kMG4005Ei10, kMHF7015 }; struct Config { explicit Config(Type type) @@ -108,6 +108,13 @@ class LkMotor { reduction_ratio = 10.0; max_torque_ = 2.5; break; + case Type::kMHF7015: + raw_angle_modulus_ = 1 << 16; + current_max = 33.0; + torque_constant = 0.51; + reduction_ratio = 1.0; + max_torque_ = 2.42; + break; default: std::unreachable(); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp new file mode 100644 index 00000000..2eeb810d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight.cpp @@ -0,0 +1,279 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "debug/rclcpp_diagnostic_log.hpp" +#include "hardware/device/bmi088.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" + +namespace rmcs_core::hardware { + +class Flight + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::agent::CBoard { +public: + Flight() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::agent::CBoard{get_parameter("board_serial").as_string()} + , flight_command_( + create_partner_component(get_component_name() + "_command", *this)) + , gimbal_yaw_motor_(*this, *flight_command_, "/gimbal/yaw") + , gimbal_pitch_motor_(*this, *flight_command_, "/gimbal/pitch") + , gimbal_left_friction_(*this, *flight_command_, "/gimbal/left_friction") + , gimbal_right_friction_(*this, *flight_command_, "/gimbal/right_friction") + , gimbal_bullet_feeder_(*this, *flight_command_, "/gimbal/bullet_feeder") + , dr16_(*this) + , bmi088_(500.0, 0.3, 0.005) { + gimbal_yaw_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMHF7015} + .set_reversed() + .set_encoder_zero_point( + static_cast(get_parameter("yaw_motor_zero_point").as_int()))); + gimbal_pitch_motor_.configure( + device::LkMotor::Config{device::LkMotor::Type::kMG4010Ei10}.set_encoder_zero_point( + static_cast(get_parameter("pitch_motor_zero_point").as_int()))); + gimbal_left_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508} + .set_reversed() + .set_reduction_ratio(1.)); + gimbal_right_friction_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM3508}.set_reduction_ratio(1.)); + gimbal_bullet_feeder_.configure( + device::DjiMotor::Config{device::DjiMotor::Type::kM2006} + .set_reversed() + .enable_multi_turn_angle()); + + register_output("/gimbal/yaw/velocity_imu", gimbal_yaw_velocity_imu_); + register_output("/gimbal/pitch/velocity_imu", gimbal_pitch_velocity_imu_); + register_output("/tf", tf_); + + bmi088_.set_coordinate_mapping( + [](double x, double y, double z) { return std::make_tuple(-x, z, y); }); + + using namespace rmcs_description; + + constexpr double rotor_distance_x = 0.83637; + constexpr double rotor_distance_y = 0.83637; + + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, rotor_distance_y / 2, 0}); + tf_->set_transform( + Eigen::Translation3d{-rotor_distance_x / 2, -rotor_distance_y / 2, 0}); + + constexpr double gimbal_center_x = 0; + constexpr double gimbal_center_y = 0; + constexpr double gimbal_center_z = -0.20552; + tf_->set_transform( + Eigen::Translation3d{gimbal_center_x, gimbal_center_y, gimbal_center_z}); + + tf_->set_transform(Eigen::Translation3d{0.0557, 0, 0.053}); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + register_output("/referee/serial", referee_serial_); + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_n( + [&buffer](std::byte byte) noexcept { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + start_transmit().uart1_transmit({ + std::span{buffer, size} + }); + return size; + }; + } + + ~Flight() override = default; + + void update() override { + update_motors(); + update_imu(); + dr16_.update_status(); + } + + void command_update() { + auto yaw_cmd = gimbal_yaw_motor_.generate_command(); + auto pitch_cmd = gimbal_pitch_motor_.generate_command(); + const auto yaw_command_count = ++yaw_can_command_count_; + const auto pitch_command_count = ++pitch_can_command_count_; + + device::CanPacket8 dji_cmds{ + gimbal_bullet_feeder_.generate_command(), device::CanPacket8::PaddingQuarter{}, + gimbal_right_friction_.generate_command(), gimbal_left_friction_.generate_command()}; + + start_transmit() + .can2_transmit({.can_id = 0x141, .can_data = yaw_cmd.as_bytes()}) + .can2_transmit({.can_id = 0x142, .can_data = pitch_cmd.as_bytes()}) + .can1_transmit({.can_id = 0x200, .can_data = dji_cmds.as_bytes()}); + + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Flight CAN command: yaw_ctrl_vel=%.6f yaw_ctrl_torque=%.6f " + "pitch_ctrl_vel=%.6f pitch_ctrl_torque=%.6f cmd_count(yaw=%llu,pitch=%llu) " + "fb_count(yaw=%llu,pitch=%llu)", + gimbal_yaw_motor_.control_velocity(), gimbal_yaw_motor_.control_torque(), + gimbal_pitch_motor_.control_velocity(), gimbal_pitch_motor_.control_torque(), + static_cast(yaw_command_count), + static_cast(pitch_command_count), + static_cast( + yaw_can_feedback_count_.load(std::memory_order_relaxed)), + static_cast( + pitch_can_feedback_count_.load(std::memory_order_relaxed))); + } + } + +private: + void update_motors() { + using namespace rmcs_description; + gimbal_yaw_motor_.update_status(); + tf_->set_state(gimbal_yaw_motor_.angle()); + + gimbal_pitch_motor_.update_status(); + tf_->set_state(gimbal_pitch_motor_.angle()); + + gimbal_bullet_feeder_.update_status(); + gimbal_left_friction_.update_status(); + gimbal_right_friction_.update_status(); + + if constexpr (RMCS_RCLCPP_DIAGNOSTIC_LOGS) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Flight CAN feedback: yaw(angle=%.6f,velocity=%.6f,torque=%.6f,count=%llu) " + "pitch(angle=%.6f,velocity=%.6f,torque=%.6f,count=%llu)", + gimbal_yaw_motor_.angle(), gimbal_yaw_motor_.velocity(), gimbal_yaw_motor_.torque(), + static_cast( + yaw_can_feedback_count_.load(std::memory_order_relaxed)), + gimbal_pitch_motor_.angle(), gimbal_pitch_motor_.velocity(), + gimbal_pitch_motor_.torque(), + static_cast( + pitch_can_feedback_count_.load(std::memory_order_relaxed))); + } + } + + void update_imu() { + bmi088_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{bmi088_.q0(), bmi088_.q1(), bmi088_.q2(), bmi088_.q3()}; + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + *gimbal_yaw_velocity_imu_ = bmi088_.gz(); + *gimbal_pitch_velocity_imu_ = bmi088_.gy(); + } + + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) {} + +protected: + void can1_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_extended_can_id || data.is_remote_transmission || data.can_data.size() < 8) + [[unlikely]] + return; + + if (data.can_id == 0x201) { + gimbal_bullet_feeder_.store_status(data.can_data); + } else if (data.can_id == 0x204) { + gimbal_left_friction_.store_status(data.can_data); + } else if (data.can_id == 0x203) { + gimbal_right_friction_.store_status(data.can_data); + } + } + + void can2_receive_callback(const librmcs::data::CanDataView& data) override { + if (data.is_remote_transmission || data.is_extended_can_id || data.can_data.size() < 8) + [[unlikely]] + return; + if (data.can_id == 0x142) { + ++pitch_can_feedback_count_; + gimbal_pitch_motor_.store_status(data.can_data); + } else if (data.can_id == 0x141) { + ++yaw_can_feedback_count_; + gimbal_yaw_motor_.store_status(data.can_data); + } + } + + void uart1_receive_callback(const librmcs::data::UartDataView& data) override { + const std::byte* ptr = data.uart_data.data(); + referee_ring_buffer_receive_.emplace_back_n( + [&ptr](std::byte* storage) noexcept { new (storage) std::byte{*ptr++}; }, + data.uart_data.size()); + } + + void dbus_receive_callback(const librmcs::data::UartDataView& data) override { + dr16_.store_status(data.uart_data.data(), data.uart_data.size()); + } + + void accelerometer_receive_callback(const librmcs::data::AccelerometerDataView& data) override { + 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); + } + +private: + class FlightCommand : public rmcs_executor::Component { + public: + explicit FlightCommand(Flight& flight) + : flight_(flight) {} + void update() override { flight_.command_update(); } + Flight& flight_; + }; + + std::shared_ptr flight_command_; + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + device::LkMotor gimbal_yaw_motor_; + device::LkMotor gimbal_pitch_motor_; + + device::DjiMotor gimbal_left_friction_; + device::DjiMotor gimbal_right_friction_; + device::DjiMotor gimbal_bullet_feeder_; + + device::Dr16 dr16_; + device::Bmi088 bmi088_; + + std::atomic yaw_can_command_count_{0}; + std::atomic pitch_can_command_count_{0}; + std::atomic yaw_can_feedback_count_{0}; + std::atomic pitch_can_feedback_count_{0}; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + + OutputInterface tf_; + + rmcs_utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::Flight, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp new file mode 100644 index 00000000..3061b9a9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/flight_mavros.cpp @@ -0,0 +1,216 @@ +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace rmcs_core::hardware { + +class FlightMavros + : public rmcs_executor::Component + , public rclcpp::Node { +public: + FlightMavros() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , tf_buffer_(get_clock()) + , tf_listener_(tf_buffer_) + , last_pose_received_time_{0, 0, get_clock()->get_clock_type()} { + load_parameters(); + + mavros_pose_publisher_ = create_publisher( + mavros_pose_topic_, rclcpp::QoS{10}.reliable()); + + RCLCPP_INFO( + logger_, + "FlightMavros bridging TF '%s' -> '%s' to '%s' with sensor RPY [%.1f, %.1f, %.1f] " + "deg.", + source_frame_id_.c_str(), target_frame_id_.c_str(), + mavros_pose_publisher_->get_topic_name(), radians_to_degrees(sensor_roll_offset_rad_), + radians_to_degrees(sensor_pitch_offset_rad_), + radians_to_degrees(sensor_yaw_offset_rad_)); + } + + ~FlightMavros() override = default; + + void update() override { + update_pose_from_tf(); + publish_credit_ += output_rate_hz_ / 1000.0; + while (publish_credit_ >= 1.0) { + publish_credit_ -= 1.0; + publish_pose(); + } + } + +private: + static double radians_to_degrees(double radians) { + return radians * 180.0 / std::numbers::pi_v; + } + + static Eigen::Quaterniond quaternion_from_rpy(double roll, double pitch, double yaw) { + const Eigen::AngleAxisd roll_rotation{roll, Eigen::Vector3d::UnitX()}; + const Eigen::AngleAxisd pitch_rotation{pitch, Eigen::Vector3d::UnitY()}; + const Eigen::AngleAxisd yaw_rotation{yaw, Eigen::Vector3d::UnitZ()}; + return Eigen::Quaterniond{roll_rotation * pitch_rotation * yaw_rotation}; + } + + void load_parameters() { + mavros_pose_topic_ = get_parameter("mavros_pose_topic").as_string(); + mavros_pose_frame_id_ = get_parameter("mavros_pose_frame_id").as_string(); + target_frame_id_ = get_parameter("target_frame_id").as_string(); + source_frame_id_ = get_parameter("source_frame_id").as_string(); + output_rate_hz_ = get_parameter("output_rate").as_double(); + sensor_roll_offset_rad_ = get_parameter("sensor_roll_offset_rad").as_double(); + sensor_pitch_offset_rad_ = get_parameter("sensor_pitch_offset_rad").as_double(); + sensor_yaw_offset_rad_ = get_parameter("sensor_yaw_offset_rad").as_double(); + + output_rate_hz_ = std::max(0.0, output_rate_hz_); + } + + Eigen::Quaterniond sensor_to_body_rotation() const { + auto sensor_to_body = quaternion_from_rpy( + sensor_roll_offset_rad_, sensor_pitch_offset_rad_, sensor_yaw_offset_rad_); + sensor_to_body.normalize(); + return sensor_to_body; + } + + static bool finite_transform(const geometry_msgs::msg::TransformStamped& transform) { + const auto& t = transform.transform.translation; + const auto& r = transform.transform.rotation; + return std::isfinite(t.x) && std::isfinite(t.y) && std::isfinite(t.z) && std::isfinite(r.x) + && std::isfinite(r.y) && std::isfinite(r.z) && std::isfinite(r.w); + } + + Eigen::Vector3d rotate_world_position(const geometry_msgs::msg::Vector3& position) const { + return Eigen::Vector3d{position.x, position.y, position.z}; + } + + Eigen::Quaterniond + rotate_body_orientation(const geometry_msgs::msg::Quaternion& orientation) const { + const Eigen::Quaterniond quat_sensor{ + orientation.w, orientation.x, orientation.y, orientation.z}; + + Eigen::Quaterniond quat_body = quat_sensor * sensor_to_body_rotation(); + quat_body.normalize(); + return quat_body; + } + + void update_pose_from_tf() { + geometry_msgs::msg::TransformStamped transform; + try { + transform = + tf_buffer_.lookupTransform(target_frame_id_, source_frame_id_, tf2::TimePointZero); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN_THROTTLE( + logger_, *get_clock(), 1000, "TF lookup failed for '%s' -> '%s': %s", + source_frame_id_.c_str(), target_frame_id_.c_str(), ex.what()); + return; + } + + if (!finite_transform(transform)) + return; + + const auto tf_stamp_ns = rclcpp::Time{transform.header.stamp}.nanoseconds(); + + std::lock_guard lock(pose_mutex_); + if (last_tf_stamp_ns_ != 0 && tf_stamp_ns <= last_tf_stamp_ns_) + return; + + const Eigen::Vector3d rotated_position = + rotate_world_position(transform.transform.translation); + const Eigen::Quaterniond rotated_orientation = + rotate_body_orientation(transform.transform.rotation); + + latest_pose_.position.x = rotated_position.x(); + latest_pose_.position.y = rotated_position.y(); + latest_pose_.position.z = rotated_position.z(); + latest_pose_.orientation.x = rotated_orientation.x(); + latest_pose_.orientation.y = rotated_orientation.y(); + latest_pose_.orientation.z = rotated_orientation.z(); + latest_pose_.orientation.w = rotated_orientation.w(); + last_tf_stamp_ns_ = tf_stamp_ns; + last_pose_received_time_ = get_clock()->now(); + pose_ready_ = true; + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Received TF '%s' -> '%s'.", source_frame_id_.c_str(), + target_frame_id_.c_str()); + } + + void publish_pose() { + geometry_msgs::msg::Pose pose; + rclcpp::Time pose_received_time{0, 0, get_clock()->get_clock_type()}; + { + std::lock_guard lock(pose_mutex_); + if (!pose_ready_) + return; + pose_received_time = last_pose_received_time_; + pose = latest_pose_; + } + + const rclcpp::Duration max_pose_age = rclcpp::Duration::from_seconds(1.0); + rclcpp::Time publish_stamp = get_clock()->now(); + if ((publish_stamp - pose_received_time) >= max_pose_age) + return; + + { + std::lock_guard lock(pose_mutex_); + if (last_published_stamp_ns_ != 0 + && publish_stamp.nanoseconds() <= last_published_stamp_ns_) { + publish_stamp = + rclcpp::Time{last_published_stamp_ns_ + 1, get_clock()->get_clock_type()}; + } + last_published_stamp_ns_ = publish_stamp.nanoseconds(); + } + + geometry_msgs::msg::PoseStamped pose_msg{}; + pose_msg.header.stamp = publish_stamp; + pose_msg.header.frame_id = mavros_pose_frame_id_; + pose_msg.pose = pose; + mavros_pose_publisher_->publish(pose_msg); + + RCLCPP_INFO_THROTTLE( + logger_, *get_clock(), 1000, "Published MAVROS vision pose at %.1f Hz to '%s'.", + output_rate_hz_, mavros_pose_publisher_->get_topic_name()); + } + + rclcpp::Logger logger_; + std::string mavros_pose_topic_; + std::string mavros_pose_frame_id_; + std::string target_frame_id_; + std::string source_frame_id_; + double output_rate_hz_; + double sensor_roll_offset_rad_; + double sensor_pitch_offset_rad_; + double sensor_yaw_offset_rad_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Publisher::SharedPtr mavros_pose_publisher_; + + std::mutex pose_mutex_; + geometry_msgs::msg::Pose latest_pose_{}; + std::int64_t last_tf_stamp_ns_{0}; + rclcpp::Time last_pose_received_time_; + std::int64_t last_published_stamp_ns_{0}; + double publish_credit_{0.0}; + bool pose_ready_{false}; +}; + +} // namespace rmcs_core::hardware + +#include +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::FlightMavros, rmcs_executor::Component)