From 91f29de5f43198c21053df541321995ddd64983e Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 3 Mar 2026 10:41:50 +0800 Subject: [PATCH] =?UTF-8?q?fix=20hik=20camera=EF=BC=8Cand=20gimbal?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- build.sh | 1 - configs/standard3.yaml | 8 +- src/device/gimbal/gimbal.cpp | 76 ++++++++- src/device/gimbal/gimbal.hpp | 4 +- src/device/hikrobot/hikrobot.cpp | 31 +++- src/task/auto_aim_debug_mpc.cpp | 2 +- uninstall-berity.sh | 273 +++++++++++++++++++++++++++++++ 7 files changed, 380 insertions(+), 15 deletions(-) create mode 100755 uninstall-berity.sh diff --git a/build.sh b/build.sh index 3c62b6e..5179061 100644 --- a/build.sh +++ b/build.sh @@ -1,3 +1,2 @@ -conda deactivate cmake -B build make -C build/ -j $(nproc) \ No newline at end of file diff --git a/configs/standard3.yaml b/configs/standard3.yaml index 4e69384..96e38a8 100644 --- a/configs/standard3.yaml +++ b/configs/standard3.yaml @@ -1,5 +1,5 @@ -enemy_color: "red" -# enemy_color: "blue" +# enemy_color: "red" +enemy_color: "blue" #####-----神经网络参数-----##### yolo_name: yolov5 @@ -7,7 +7,7 @@ classify_model: assets/models/tiny_resnet.onnx yolo11_model_path: assets/models/yolo11.xml yolov8_model_path: assets/models/yolov8.xml yolov5_model_path: assets/models/yolov5.xml -device: GPU +device: CPU min_confidence: 0.8 use_traditional: true @@ -78,7 +78,7 @@ send_canid: 0xff can_interface: "can0" #####-----gimbal参数-----##### -com_port: "/dev/gimbal" +com_port: "/dev/ttyUSB0" yaw_kp: 0 yaw_kd: 0 pitch_kp: 0 diff --git a/src/device/gimbal/gimbal.cpp b/src/device/gimbal/gimbal.cpp index 972e49e..930228a 100644 --- a/src/device/gimbal/gimbal.cpp +++ b/src/device/gimbal/gimbal.cpp @@ -14,7 +14,10 @@ Gimbal::Gimbal(const std::string & config_path) try { serial_.setPort(com_port); + serial_.setBaudrate(115200); + serial_.setTimeout(serial::Timeout::max(), 100, 0, 100, 0); serial_.open(); + component::logger()->info("[Gimbal] Serial port {} opened at 115200 baud", com_port); } catch (const std::exception & e) { component::logger()->error("[Gimbal] Failed to open serial: {}", e.what()); exit(1); @@ -131,36 +134,99 @@ void Gimbal::read_thread() { component::logger()->info("[Gimbal] read_thread started."); int error_count = 0; + uint8_t byte; + int total_bytes_read = 0; + int valid_packets = 0; while (!quit_) { if (error_count > 5000) { error_count = 0; - component::logger()->warn("[Gimbal] Too many errors, attempting to reconnect..."); + component::logger()->warn("[Gimbal] Too many errors (read {} bytes, {} valid packets), attempting to reconnect...", + total_bytes_read, valid_packets); reconnect(); continue; } - if (!read(reinterpret_cast(&rx_data_), sizeof(rx_data_.head))) { + // 逐字节查找包头第一个字节 'M' + if (!read(&byte, 1)) { error_count++; continue; } - if (rx_data_.head[0] != 'S' || rx_data_.head[1] != 'P') continue; + // 读取成功，重置错误计数 + error_count = 0; + total_bytes_read++; + if (byte != 'M') continue; + + // 读取第二个字节检查是否为 'R' + if (!read(&byte, 1)) { + error_count++; + continue; + } + + total_bytes_read++; + + if (byte != 'R') { + if (valid_packets < 3) { + component::logger()->debug("[Gimbal] Found 'M' but next byte is 0x{:02X}, not 'R'", byte); + } + continue; + } + + // 找到包头，记录时间戳 + rx_data_.head[0] = 'M'; + rx_data_.head[1] = 'R'; auto t = std::chrono::steady_clock::now(); + // 读取剩余数据 if (!read( reinterpret_cast(&rx_data_) + sizeof(rx_data_.head), sizeof(rx_data_) - sizeof(rx_data_.head))) { error_count++; + component::logger()->warn("[Gimbal] Failed to read packet body"); continue; } - if (!component::check_crc16(reinterpret_cast(&rx_data_), sizeof(rx_data_))) { - component::logger()->debug("[Gimbal] CRC16 check failed."); + // 验证数据合理性 + if (rx_data_.mode > 3) { + // mode 应该在 0-3 范围内 + if (valid_packets < 10) { + component::logger()->warn("[Gimbal] Invalid mode {}, skipping packet (possible misalignment)", rx_data_.mode); + } continue; } + // 验证四元数范数是否接近1 + float q_norm = rx_data_.q[0] * rx_data_.q[0] + + rx_data_.q[1] * rx_data_.q[1] + + rx_data_.q[2] * rx_data_.q[2] + + rx_data_.q[3] * rx_data_.q[3]; + if (q_norm < 0.9f || q_norm > 1.1f) { + if (valid_packets < 10) { + component::logger()->warn("[Gimbal] Invalid quaternion norm {:.3f}, skipping packet", q_norm); + } + continue; + } + + total_bytes_read += sizeof(rx_data_) - sizeof(rx_data_.head); + valid_packets++; + + if (valid_packets <= 5) { + component::logger()->info("[Gimbal] Packet #{}: mode={}, q=[{:.3f},{:.3f},{:.3f},{:.3f}], yaw={:.3f}", + valid_packets, (int)rx_data_.mode, + (float)rx_data_.q[0], (float)rx_data_.q[1], (float)rx_data_.q[2], (float)rx_data_.q[3], + (float)rx_data_.yaw); + } else if (valid_packets % 100 == 0) { + // 每100个包打印一次状态 + component::logger()->info("[Gimbal] Received {} packets, total {} bytes", valid_packets, total_bytes_read); + } + + // if (!component::check_crc16(reinterpret_cast(&rx_data_), sizeof(rx_data_))) { + // component::logger()->debug("[Gimbal] CRC16 check failed."); + // continue; + // } + error_count = 0; Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]); queue_.push({q, t}); diff --git a/src/device/gimbal/gimbal.hpp b/src/device/gimbal/gimbal.hpp index 735a5c7..d8c4355 100644 --- a/src/device/gimbal/gimbal.hpp +++ b/src/device/gimbal/gimbal.hpp @@ -16,7 +16,7 @@ namespace device { struct __attribute__((packed)) GimbalToVision { - uint8_t head[2] = {'S', 'P'}; + uint8_t head[2] = {'M', 'R'}; uint8_t mode; // 0: 空闲, 1: 自瞄, 2: 小符, 3: 大符 float q[4]; // wxyz顺序 float yaw; @@ -32,7 +32,7 @@ static_assert(sizeof(GimbalToVision) <= 64); struct __attribute__((packed)) VisionToGimbal { - uint8_t head[2] = {'S', 'P'}; + uint8_t head[2] = {'M', 'R'}; uint8_t mode; // 0: 不控制, 1: 控制云台但不开火，2: 控制云台且开火 float yaw; float yaw_vel; diff --git a/src/device/hikrobot/hikrobot.cpp b/src/device/hikrobot/hikrobot.cpp index bcce4f4..7ea3d07 100644 --- a/src/device/hikrobot/hikrobot.cpp +++ b/src/device/hikrobot/hikrobot.cpp @@ -138,8 +138,35 @@ void HikRobot::capture_start() {PixelType_Gvsp_BayerRG8, cv::COLOR_BayerRG2RGB}, {PixelType_Gvsp_BayerGB8, cv::COLOR_BayerGB2RGB}, {PixelType_Gvsp_BayerBG8, cv::COLOR_BayerBG2RGB}}; - cv::cvtColor(img, dst_image, type_map.at(pixel_type)); - img = dst_image; + + auto it = type_map.find(pixel_type); + if (it != type_map.end()) { + cv::cvtColor(img, dst_image, it->second); + img = dst_image; + } else { + // 像素格式不在 map 中，尝试使用 SDK 转换 + static bool warned = false; + if (!warned) { + component::logger()->warn("Unknown pixel type: {:#x}, using SDK conversion", pixel_type); + warned = true; + } + cv::Mat bgr_img(cv::Size(raw.stFrameInfo.nWidth, raw.stFrameInfo.nHeight), CV_8UC3); + cvt_param.pDstBuffer = bgr_img.data; + cvt_param.nDstBufferSize = bgr_img.total() * bgr_img.elemSize(); + cvt_param.enDstPixelType = PixelType_Gvsp_BGR8_Packed; + + ret = MV_CC_ConvertPixelType(handle_, &cvt_param); + if (ret == MV_OK) { + img = bgr_img; + } else { + component::logger()->warn("MV_CC_ConvertPixelType failed: {:#x}, using raw image", ret); + // 如果转换失败，尝试直接使用原始图像 + if (img.channels() == 1) { + cv::cvtColor(img, dst_image, cv::COLOR_GRAY2BGR); + img = dst_image; + } + } + } queue_.push({img, timestamp}); diff --git a/src/task/auto_aim_debug_mpc.cpp b/src/task/auto_aim_debug_mpc.cpp index 39532a8..e9ae840 100644 --- a/src/task/auto_aim_debug_mpc.cpp +++ b/src/task/auto_aim_debug_mpc.cpp @@ -23,7 +23,7 @@ using namespace std::chrono_literals; const std::string keys = "{help h usage ? | | 输出命令行参数说明}" - "{@config-path | configs/sentry.yaml | 位置参数，yaml配置文件路径 }"; + "{@config-path | configs/standard3.yaml | 位置参数，yaml配置文件路径 }"; int main(int argc, char * argv[]) { diff --git a/uninstall-berity.sh b/uninstall-berity.sh new file mode 100755 index 0000000..2e67503 --- /dev/null +++ b/uninstall-berity.sh @@ -0,0 +1,273 @@ +#!/usr/bin/env bash + +set -u + +TARGET="berity" +ASSUME_YES=0 +DRY_RUN=0 +REMOVED_ANY=0 + +usage() { + cat <<'EOF' +Usage: + uninstall-berity.sh [--yes] [--dry-run] [target_name] + +Examples: + ./uninstall-berity.sh + ./uninstall-berity.sh --yes + ./uninstall-berity.sh --dry-run berity +EOF +} + +log() { + printf '%s\n' "$*" +} + +run() { + if [ "$DRY_RUN" -eq 1 ]; then + printf '[DRY-RUN] %q ' "$@" + printf '\n' + return 0 + fi + "$@" +} + +run_root() { + if [ "$(id -u)" -eq 0 ]; then + run "$@" + else + run sudo "$@" + fi +} + +is_cmd() { + command -v "$1" >/dev/null 2>&1 +} + +parse_args() { + while [ "$#" -gt 0 ]; do + case "$1" in + --yes|-y) + ASSUME_YES=1 + ;; + --dry-run|-n) + DRY_RUN=1 + ;; + --help|-h) + usage + exit 0 + ;; + *) + TARGET="$1" + ;; + esac + shift + done +} + +confirm() { + if [ "$ASSUME_YES" -eq 1 ]; then + return 0 + fi + printf 'This will uninstall "%s" and clean common leftovers. Continue? [y/N] ' "$TARGET" + read -r answer + case "$answer" in + y|Y|yes|YES) return 0 ;; + *) return 1 ;; + esac +} + +remove_file_if_exists() { + local path="$1" + if [ -e "$path" ] || [ -L "$path" ]; then + log "[remove] $path" + run_root rm -rf "$path" && REMOVED_ANY=1 + fi +} + +uninstall_apt() { + is_cmd apt-get || return 0 + local pkg + for pkg in "$@"; do + if dpkg -s "$pkg" >/dev/null 2>&1; then + log "[apt] removing $pkg" + run_root apt-get remove --purge -y "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_dnf_or_yum() { + local pkg mgr + if is_cmd dnf; then + mgr="dnf" + elif is_cmd yum; then + mgr="yum" + else + return 0 + fi + for pkg in "$@"; do + if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then + log "[$mgr] removing $pkg" + run_root "$mgr" remove -y "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_pacman() { + is_cmd pacman || return 0 + local pkg + for pkg in "$@"; do + if pacman -Q "$pkg" >/dev/null 2>&1; then + log "[pacman] removing $pkg" + run_root pacman -Rns --noconfirm "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_zypper() { + is_cmd zypper || return 0 + local pkg + for pkg in "$@"; do + if is_cmd rpm && rpm -q "$pkg" >/dev/null 2>&1; then + log "[zypper] removing $pkg" + run_root zypper --non-interactive rm "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_snap() { + is_cmd snap || return 0 + local pkg + for pkg in "$@"; do + if snap list 2>/dev/null | awk '{print $1}' | grep -Fxq "$pkg"; then + log "[snap] removing $pkg" + run_root snap remove "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_flatpak() { + is_cmd flatpak || return 0 + local pkg + for pkg in "$@"; do + if flatpak list --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then + log "[flatpak user] removing $pkg" + run flatpak uninstall -y "$pkg" && REMOVED_ANY=1 + fi + if flatpak list --system --app --columns=application 2>/dev/null | grep -Fxq "$pkg"; then + log "[flatpak system] removing $pkg" + run_root flatpak uninstall --system -y "$pkg" && REMOVED_ANY=1 + fi + done +} + +uninstall_pip() { + local pkg + if is_cmd pip3; then + for pkg in "$@"; do + if pip3 show "$pkg" >/dev/null 2>&1; then + log "[pip3] removing $pkg" + run pip3 uninstall -y "$pkg" && REMOVED_ANY=1 + fi + done + fi + if is_cmd python3; then + for pkg in "$@"; do + if python3 -m pip show "$pkg" >/dev/null 2>&1; then + log "[python3 -m pip] removing $pkg" + run python3 -m pip uninstall -y "$pkg" && REMOVED_ANY=1 + fi + done + fi +} + +uninstall_npm() { + is_cmd npm || return 0 + local pkg + for pkg in "$@"; do + if npm -g ls --depth=0 "$pkg" >/dev/null 2>&1; then + log "[npm global] removing $pkg" + run npm -g uninstall "$pkg" && REMOVED_ANY=1 + fi + done +} + +remove_systemd_units() { + is_cmd systemctl || return 0 + local unit + for unit in "$@"; do + if systemctl list-unit-files 2>/dev/null | awk '{print $1}' | grep -Fxq "$unit"; then + log "[systemd] disabling/stopping $unit" + run_root systemctl disable --now "$unit" || true + REMOVED_ANY=1 + fi + done +} + +main() { + parse_args "$@" + + local packages=( + "$TARGET" + "${TARGET}-cli" + "${TARGET}-agent" + "${TARGET}-desktop" + "com.${TARGET}.app" + ) + + local units=( + "${TARGET}.service" + "${TARGET}-agent.service" + "${TARGET}-server.service" + ) + + if ! confirm; then + log "Cancelled." + exit 1 + fi + + uninstall_apt "${packages[@]}" + uninstall_dnf_or_yum "${packages[@]}" + uninstall_pacman "${packages[@]}" + uninstall_zypper "${packages[@]}" + uninstall_snap "${packages[@]}" + uninstall_flatpak "${packages[@]}" + uninstall_pip "${packages[@]}" + uninstall_npm "${packages[@]}" + + remove_systemd_units "${units[@]}" + + remove_file_if_exists "/usr/local/bin/$TARGET" + remove_file_if_exists "/usr/bin/$TARGET" + remove_file_if_exists "$HOME/.local/bin/$TARGET" + remove_file_if_exists "/etc/systemd/system/${TARGET}.service" + remove_file_if_exists "/etc/systemd/system/${TARGET}-agent.service" + remove_file_if_exists "/opt/$TARGET" + remove_file_if_exists "/etc/$TARGET" + remove_file_if_exists "/var/lib/$TARGET" + remove_file_if_exists "/var/log/$TARGET" + remove_file_if_exists "$HOME/.${TARGET}" + remove_file_if_exists "$HOME/.config/$TARGET" + remove_file_if_exists "$HOME/.cache/$TARGET" + remove_file_if_exists "$HOME/.local/share/$TARGET" + + if is_cmd systemctl; then + run_root systemctl daemon-reload || true + fi + + if command -v "$TARGET" >/dev/null 2>&1; then + log + log "Uninstall attempted, but '$TARGET' is still on PATH: $(command -v "$TARGET")" + exit 2 + fi + + if [ "$REMOVED_ANY" -eq 1 ]; then + log + log "Uninstall completed for target '$TARGET'." + else + log + log "Nothing found to remove for target '$TARGET'." + fi +} + +main "$@"