VSAG/vision_sjtu_19
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

1

Browse Source
This commit is contained in:
Luo Yufei
2026-03-28 04:54:33 +08:00
parent f40c37c136
commit e8d6bbac42
6 changed files with 53 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@@ -28,7 +28,6 @@ ENDIF()
FIND_PACKAGE(Eigen3 REQUIRED) FIND_PACKAGE(Eigen3 REQUIRED)
FIND_PACKAGE(OpenCV 4 REQUIRED) FIND_PACKAGE(OpenCV 4 REQUIRED)
FIND_PACKAGE(Threads) FIND_PACKAGE(Threads)
FIND_PACKAGE(fmt REQUIRED)
LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others) LINK_DIRECTORIES(${PROJECT_SOURCE_DIR}/others)
@@ -42,7 +41,6 @@ ADD_EXECUTABLE(${BIN_NAME} main.cpp ${sourcefiles})
TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT}) TARGET_LINK_LIBRARIES(${BIN_NAME} ${CMAKE_THREAD_LIBS_INIT})
TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS}) TARGET_LINK_LIBRARIES(${BIN_NAME} ${OpenCV_LIBS})
TARGET_LINK_LIBRARIES(${BIN_NAME} fmt::fmt)
IF (CMAKE_SYSTEM_NAME MATCHES "Linux") IF (CMAKE_SYSTEM_NAME MATCHES "Linux")
MESSAGE(STATUS "current platform: Linux ") MESSAGE(STATUS "current platform: Linux ")
TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so") TARGET_LINK_LIBRARIES(${BIN_NAME} "${PROJECT_SOURCE_DIR}/others/libMVSDK.so")

2
armor/include/serialComm.hpp
View File

@@ -4,7 +4,7 @@
#include <vector> #include <vector>
#include "CSerialPort/SerialPort.h" #include "CSerialPort/SerialPort.h"
#include "CSerialPort/SerialPortInfo.h" #include "CSerialPort/SerialPortInfo.h"
#include <fmt/core.h> #include <cstdio>
using namespace itas109; using namespace itas109;

14
armor/include/serialManager.hpp
View File

@@ -5,7 +5,7 @@
#include <thread> #include <thread>
#include <atomic> #include <atomic>
#include <mutex> #include <mutex>
#include <fmt/core.h> #include <cstdio>
#include <chrono> #include <chrono>
class SerialManager { class SerialManager {
@@ -19,7 +19,7 @@ private:
// 后台重连线程函数 // 后台重连线程函数
void connectionThreadFunc() { void connectionThreadFunc() {
fmt::print("[I][SerialMgr]: RETRY\n"); printf("[I][SerialMgr]: RETRY\n");
while (!m_shouldStop.load()) { while (!m_shouldStop.load()) {
if (!m_isConnected.load()) { if (!m_isConnected.load()) {
@@ -29,14 +29,14 @@ private:
if (m_serial.findFirstTtyUSB() && m_serial.openPort()) { if (m_serial.findFirstTtyUSB() && m_serial.openPort()) {
m_isConnected.store(true); m_isConnected.store(true);
} else { } else {
fmt::print("[W][SerialMgr]: Failed, retry in {}ms\n", m_retryIntervalMs); printf("[W][SerialMgr]: Failed, retry in %dms\n", m_retryIntervalMs);
} }
} }
std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs)); std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs));
} }
fmt::print("[I][SerialMgr]: EXIT\n"); printf("[I][SerialMgr]: EXIT\n");
} }
public: public:
@@ -56,7 +56,7 @@ public:
if (!m_connectionThread.joinable()) { if (!m_connectionThread.joinable()) {
m_shouldStop.store(false); m_shouldStop.store(false);
m_connectionThread = std::thread(&SerialManager::connectionThreadFunc, this); m_connectionThread = std::thread(&SerialManager::connectionThreadFunc, this);
fmt::print("[I][SerialMgr]: START\n"); printf("[I][SerialMgr]: START\n");
} }
} }
@@ -70,7 +70,7 @@ public:
m_serial.closePort(); m_serial.closePort();
m_isConnected.store(false); m_isConnected.store(false);
fmt::print("[I][SerialMgr]: STOP\n"); printf("[I][SerialMgr]: STOP\n");
} }
} }
@@ -90,7 +90,7 @@ public:
if (!m_serial.sendData(data, length)) { if (!m_serial.sendData(data, length)) {
// 发送失败，标记为断开 // 发送失败，标记为断开
m_isConnected.store(false); m_isConnected.store(false);
fmt::print("[W][SerialMgr]: Failed, mark DISCONNECT\n"); printf("[W][SerialMgr]: Failed, mark DISCONNECT\n");
return false; return false;
} }

1
armor/include/unifiedManager.hpp
View File

@@ -7,7 +7,6 @@
#include <atomic> #include <atomic>
#include <mutex> #include <mutex>
#include <chrono> #include <chrono>
#include <fmt/core.h>
// Unified device manager (Serial only) // Unified device manager (Serial only)
class UnifiedDeviceManager { class UnifiedDeviceManager {

68
armor/src/serialComm.cpp
View File

@@ -1,11 +1,10 @@
#include "serialComm.hpp" #include "serialComm.hpp"
#include <fmt/format.h>
#include <cstring> #include <cstring>
#include <cstdio>
SerialComm::SerialComm() : m_isOpen(false) { SerialComm::SerialComm() : m_isOpen(false) {
std::memset(m_portName, 0, sizeof(m_portName)); std::memset(m_portName, 0, sizeof(m_portName));
// Initialize serial port parameters
m_serialPort.init( m_serialPort.init(
"", "",
SerialConfig::BAUD_RATE, SerialConfig::BAUD_RATE,
@@ -23,29 +22,26 @@ SerialComm::~SerialComm() {
bool SerialComm::findFirstTtyUSB() { bool SerialComm::findFirstTtyUSB() {
std::vector<SerialPortInfo> portInfos = CSerialPortInfo::availablePortInfos(); std::vector<SerialPortInfo> portInfos = CSerialPortInfo::availablePortInfos();
for (const auto& info : portInfos) { for (const auto& info : portInfos) {
// 查找第一个 ttyUSB 设备
const char* portStr = info.portName; const char* portStr = info.portName;
if (std::strstr(portStr, "ttyCH341USB") != nullptr) { if (std::strstr(portStr, "ttyCH341USB") != nullptr) {
std::strncpy(m_portName, portStr, sizeof(m_portName) - 1); std::strncpy(m_portName, portStr, sizeof(m_portName) - 1);
m_portName[sizeof(m_portName) - 1] = '\0'; // 字符串结束 m_portName[sizeof(m_portName) - 1] = '\0';
fmt::print("[I][SERIAL]: ttyUSB device found: {}\n", m_portName); printf("[I][SERIAL]: ttyUSB device found: %s\n", m_portName);
return true; return true;
} }
} }
return false; return false;
} }
bool SerialComm::openPort() { bool SerialComm::openPort() {
if (std::strlen(m_portName) == 0) { if (std::strlen(m_portName) == 0) {
fmt::print("[E][Serial]: Port name is empty, call findFirstTtyUSB() first\n"); printf("[E][Serial]: Port name is empty, call findFirstTtyUSB() first\n");
return false; return false;
} }
// Set port name
m_serialPort.init( m_serialPort.init(
m_portName, m_portName,
SerialConfig::BAUD_RATE, SerialConfig::BAUD_RATE,
@@ -55,21 +51,21 @@ bool SerialComm::openPort() {
SerialConfig::FLOW_CTRL, SerialConfig::FLOW_CTRL,
SerialConfig::READ_TIMEOUT_MS SerialConfig::READ_TIMEOUT_MS
); );
if (!m_serialPort.open()) { if (!m_serialPort.open()) {
fmt::print("[E][Serial]: Failed to open port {}\n", m_portName); printf("[E][Serial]: Failed to open port %s\n", m_portName);
fmt::print("[E][Serial]: Error code: {}\n", m_serialPort.getLastError()); printf("[E][Serial]: Error code: %d\n", m_serialPort.getLastError());
m_isOpen = false; m_isOpen = false;
return false; return false;
} }
m_isOpen = true; m_isOpen = true;
fmt::print("[I][Serial]: Port opened successfully: {}\n", m_portName); printf("[I][Serial]: Port opened successfully: %s\n", m_portName);
fmt::print("[I][Serial]: Baud rate: {}\n", SerialConfig::BAUD_RATE); printf("[I][Serial]: Baud rate: %d\n", SerialConfig::BAUD_RATE);
fmt::print("[I][Serial]: Data bits: 8\n"); printf("[I][Serial]: Data bits: 8\n");
fmt::print("[I][Serial]: Stop bits: 1\n"); printf("[I][Serial]: Stop bits: 1\n");
fmt::print("[I][Serial]: Parity: None\n"); printf("[I][Serial]: Parity: None\n");
return true; return true;
} }
@@ -77,7 +73,7 @@ void SerialComm::closePort() {
if (m_isOpen) { if (m_isOpen) {
m_serialPort.close(); m_serialPort.close();
m_isOpen = false; m_isOpen = false;
fmt::print("[I][Serial]: Port closed: {}\n", m_portName); printf("[I][Serial]: Port closed: %s\n", m_portName);
} }
} }
@@ -87,17 +83,17 @@ bool SerialComm::sendData(const char* data, size_t length) {
bool SerialComm::sendData(const uint8_t* data, size_t length) { bool SerialComm::sendData(const uint8_t* data, size_t length) {
if (!m_isOpen) { if (!m_isOpen) {
fmt::print("[E][Serial]: Port not open\n"); printf("[E][Serial]: Port not open\n");
return false; return false;
} }
int bytesWritten = m_serialPort.writeData(data, length); int bytesWritten = m_serialPort.writeData(data, length);
if (bytesWritten > 0) { if (bytesWritten > 0) {
fmt::print("[I][Serial]: Sent {} bytes\n", bytesWritten); printf("[I][Serial]: Sent %d bytes\n", bytesWritten);
return true; return true;
} else { } else {
fmt::print("[E][Serial]: Failed to send data\n"); printf("[E][Serial]: Failed to send data\n");
return false; return false;
} }
} }
@@ -108,24 +104,24 @@ int SerialComm::receiveData(char* buffer, size_t maxLength) {
int SerialComm::receiveData(uint8_t* buffer, size_t maxLength) { int SerialComm::receiveData(uint8_t* buffer, size_t maxLength) {
if (!m_isOpen) { if (!m_isOpen) {
fmt::print("[E][Serial]: Port not open\n"); printf("[E][Serial]: Port not open\n");
return -1; return -1;
} }
int bytesRead = m_serialPort.readData(buffer, maxLength); int bytesRead = m_serialPort.readData(buffer, maxLength);
if (bytesRead > 0) { if (bytesRead > 0) {
fmt::print("[I][Serial]: Received {} bytes\n", bytesRead); printf("[I][Serial]: Received %d bytes\n", bytesRead);
} }
return bytesRead; return bytesRead;
} }
void SerialComm::listAllPorts() { void SerialComm::listAllPorts() {
std::vector<SerialPortInfo> portInfos = CSerialPortInfo::availablePortInfos(); std::vector<SerialPortInfo> portInfos = CSerialPortInfo::availablePortInfos();
fmt::print("[I][Serial]: Available ports:\n"); printf("[I][Serial]: Available ports:\n");
for (const auto& info : portInfos) { for (const auto& info : portInfos) {
fmt::print("[I][Serial]: - {}\n", info.portName); printf("[I][Serial]: - %s\n", info.portName);
} }
} }

25
armor/src/unifiedManager.cpp
View File

@@ -1,4 +1,5 @@
#include "unifiedManager.hpp" #include "unifiedManager.hpp"
#include <cstdio>
UnifiedDeviceManager::UnifiedDeviceManager(int retryIntervalMs) UnifiedDeviceManager::UnifiedDeviceManager(int retryIntervalMs)
: m_serial(std::make_unique<SerialComm>()) : m_serial(std::make_unique<SerialComm>())
@@ -6,7 +7,7 @@ UnifiedDeviceManager::UnifiedDeviceManager(int retryIntervalMs)
, m_shouldStop(false) , m_shouldStop(false)
, m_retryIntervalMs(retryIntervalMs) , m_retryIntervalMs(retryIntervalMs)
{ {
fmt::print("[I][Managr]: Unified device manager created\n"); printf("[I][Managr]: Unified device manager created\n");
} }
UnifiedDeviceManager::~UnifiedDeviceManager() { UnifiedDeviceManager::~UnifiedDeviceManager() {
@@ -14,31 +15,31 @@ UnifiedDeviceManager::~UnifiedDeviceManager() {
} }
void UnifiedDeviceManager::serialReconnectThreadFunc() { void UnifiedDeviceManager::serialReconnectThreadFunc() {
fmt::print("[I][Managr]: Serial reconnect thread started\n"); printf("[I][Managr]: Serial reconnect thread started\n");
while (!m_shouldStop.load()) { while (!m_shouldStop.load()) {
if (!m_serialConnected.load()) { if (!m_serialConnected.load()) {
fmt::print("[I][Managr]: Attempting to connect serial port...\n"); printf("[I][Managr]: Attempting to connect serial port...\n");
std::lock_guard<std::mutex> lock(m_serialMutex); std::lock_guard<std::mutex> lock(m_serialMutex);
if (m_serial->findFirstTtyUSB() && m_serial->openPort()) { if (m_serial->findFirstTtyUSB() && m_serial->openPort()) {
m_serialConnected.store(true); m_serialConnected.store(true);
fmt::print("[I][Managr]: Serial port connected successfully\n"); printf("[I][Managr]: Serial port connected successfully\n");
} else { } else {
fmt::print("[W][Managr]: Serial connection failed, retry in {}ms\n", printf("[W][Managr]: Serial connection failed, retry in %dms\n",
m_retryIntervalMs); m_retryIntervalMs);
} }
} }
std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs)); std::this_thread::sleep_for(std::chrono::milliseconds(m_retryIntervalMs));
} }
fmt::print("[I][Managr]: Serial reconnect thread stopped\n"); printf("[I][Managr]: Serial reconnect thread stopped\n");
} }
void UnifiedDeviceManager::start() { void UnifiedDeviceManager::start() {
fmt::print("[I][Managr]: Starting unified device manager\n"); printf("[I][Managr]: Starting unified device manager\n");
m_shouldStop.store(false); m_shouldStop.store(false);
@@ -46,11 +47,11 @@ void UnifiedDeviceManager::start() {
m_serialReconnectThread = std::thread(&UnifiedDeviceManager::serialReconnectThreadFunc, this); m_serialReconnectThread = std::thread(&UnifiedDeviceManager::serialReconnectThreadFunc, this);
} }
fmt::print("[I][Managr]: Unified device manager started\n"); printf("[I][Managr]: Unified device manager started\n");
} }
void UnifiedDeviceManager::stop() { void UnifiedDeviceManager::stop() {
fmt::print("[I][Managr]: Stopping unified device manager\n"); printf("[I][Managr]: Stopping unified device manager\n");
m_shouldStop.store(true); m_shouldStop.store(true);
@@ -62,7 +63,7 @@ void UnifiedDeviceManager::stop() {
m_serialConnected.store(false); m_serialConnected.store(false);
} }
fmt::print("[I][Managr]: Unified device manager stopped\n"); printf("[I][Managr]: Unified device manager stopped\n");
} }
bool UnifiedDeviceManager::sendData(const char* data, size_t length) { bool UnifiedDeviceManager::sendData(const char* data, size_t length) {
@@ -74,7 +75,7 @@ bool UnifiedDeviceManager::sendData(const char* data, size_t length) {
if (!m_serial->sendData(data, length)) { if (!m_serial->sendData(data, length)) {
m_serialConnected.store(false); m_serialConnected.store(false);
fmt::print("[W][Managr]: Serial send failed, marked as disconnected\n"); printf("[W][Managr]: Serial send failed, marked as disconnected\n");
return false; return false;
} }