robofish/MOVE_AI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix hik camera，and gimbal

Browse Source
This commit is contained in:
Robofish 2026-03-03 10:41:50 +08:00
parent 5c208f7604
commit 91f29de5f4
7 changed files with 380 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
build.sh
View File

@ -1,3 +1,2 @@
conda deactivate
cmake -B build
make -C build/ -j $(nproc)

8
configs/standard3.yaml
View File

@ -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

76
src/device/gimbal/gimbal.cpp
View File

@ -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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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<uint8_t *>(&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});

4
src/device/gimbal/gimbal.hpp
View File

@ -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;

29
src/device/hikrobot/hikrobot.cpp
View File

@ -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));
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});

2
src/task/auto_aim_debug_mpc.cpp
View File

@ -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[])
{

273
uninstall-berity.sh Executable file
View File

@ -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 "$@"