diff --git a/backup/demo.cpp b/backup/demo.cpp deleted file mode 100644 index e68c4d2..0000000 --- a/backup/demo.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// #include "armcontroller.hpp" -// #include -// #include - -// #include -// #include -// #include -// #include - -// string urdfPath = "/home/unitree/unitreeApp/urdf/d1_description/urdf/d1_description.urdf"; - -// int main() -// { -// armController *arm = new armController(); - -// double x, y, z, row, pitch, yaw; - -// std::this_thread::sleep_for(std::chrono::seconds(1)); - -// arm->trashActionDefault(); -// arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); -// // arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// // arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); - -// // return 0; -// std::this_thread::sleep_for(std::chrono::seconds(3)); - -// arm->goHome(); -// arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); - -// std::this_thread::sleep_for(std::chrono::seconds(3)); - -// // arm->ikPoseRPY(x, y, z, row, pitch, yaw); - -// std::this_thread::sleep_for(std::chrono::seconds(5)); - -// bool flag = true; -// while (flag) -// { -// char ch; -// cin >> ch; -// switch (ch) -// { -// case 'w': -// x += 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 'a': -// y += 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 's': -// x -= 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 'd': -// y -= 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 'i': -// z += 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 'k': -// z -= 0.05; -// arm->ikPoseRPY(x, y, z, row, pitch, yaw); -// break; -// case 'c': -// arm->goHome(); -// arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); -// break; -// case 'p': -// arm->ikPoseRPY(0.462213, -0.00559882, -0.0217322, 3.1036, 0.151366, 3.04644); -// arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); -// break; -// case 'q': -// flag = false; -// } -// } -// return 0; -// } \ No newline at end of file diff --git a/include/armcontroller.hpp b/include/armcontroller.hpp index 907208c..e4a4df4 100644 --- a/include/armcontroller.hpp +++ b/include/armcontroller.hpp @@ -14,6 +14,7 @@ #include //位姿工具提供 #include //前向运动学 #include //逆向运动学 +#include #define TOPIC "rt/arm_Command" #define TOPICSTATUS "current_servo_angle" @@ -60,10 +61,10 @@ public: // 从urdf构建base->tip的kdl::chain,并返回关节顺序(后续角度填写顺序) bool buildChainFromUrdf(const string &urdfPath, const string &baseLink, const string &tipLink, KDL::Chain &outChain, vector &outJointNames); - bool ikPoseRPY(double x,double y,double z,double roll,double pitch,double yaw); + bool ikPoseRPY(double x, double y, double z, double roll, double pitch, double yaw); // 根据角度求坐标 - bool getCurrentPoseFromMsg(double &x, double &y, double &z, double &roll, double &pitch, double &yaw); + bool getCurrentPoseFromMsg(double &x,double &y,double &z,double &roll,double &pitch,double &yaw); // 填充至JntArray void fillToJntArray(const msg_data &msgdata_, KDL::JntArray &qSol); @@ -72,14 +73,20 @@ public: void fillToMsgData(const KDL::JntArray &qSol, msg_data &msgdata); ///////基本控制/////// - // 夹爪前进 - void goForward(); + + void goAction(const double x,const double y,const double z,const double roll,const double pitch,const double yaw); + + // 电机动作 + void angleLRAction(double lr); // 夹爪后退 - void goBack(); + void angleUpDownAction(double j1,double j2,double j4); // 夹爪向上 - void goUp(); + void angleRotateAction(double j5); + + //扔垃圾 + void throwTrash(); // 夹爪向下 void goDown(); @@ -109,6 +116,7 @@ public: private: // 发送消息 msg_data msgData_{}; + // anglePos anglePos_{}; uint seq = 0; // 唯一识别码 uint funcdoe = 0; // 指令功能码 uint address = 0; // 指令地址码 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 8d7d451..ff6138f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -39,9 +39,14 @@ set(SRC_COMMON MySerial.cpp ) +# 机械臂 DDS 消息源码（用于 ArmString_ 类型序列化） +set(ARM_MSG_SRC ${PROJ_ROOT}/arm/src/msg/ArmString_.cpp) + # 可执行程序：keybordControl（入口 main.cpp） add_executable(keybordControl main.cpp + armController.cpp + ${ARM_MSG_SRC} ${SRC_COMMON} ) @@ -55,6 +60,27 @@ target_link_libraries(keybordControl rt ) +# ========== 链接 KDL/kdl_parser/urdfdom 等（armController 需要） ========== +find_library(OROCOS_KDL_LIB NAMES orocos-kdl HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) +find_library(KDL_PARSER_LIB NAMES kdl_parser HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) +find_library(URDFDOM_MODEL_LIB NAMES urdfdom_model HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) +find_library(URDFDOM_WORLD_LIB NAMES urdfdom_world HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) +find_library(CONSOLE_BRIDGE_LIB NAMES console_bridge HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) +find_library(TINYXML2_LIB NAMES tinyxml2 HINTS /usr/lib /usr/lib/aarch64-linux-gnu /usr/local/lib) + +if(OROCOS_KDL_LIB AND KDL_PARSER_LIB AND URDFDOM_MODEL_LIB AND URDFDOM_WORLD_LIB AND CONSOLE_BRIDGE_LIB AND TINYXML2_LIB) + target_link_libraries(keybordControl + ${KDL_PARSER_LIB} + ${OROCOS_KDL_LIB} + ${URDFDOM_MODEL_LIB} + ${URDFDOM_WORLD_LIB} + ${CONSOLE_BRIDGE_LIB} + ${TINYXML2_LIB} + ) +else() + message(WARNING "KDL/kdl_parser/urdfdom libraries not found. If armController uses them, please install related -dev packages.") +endif() + # 构建信息 message(STATUS "OpenCV libs: ${OpenCV_LIBS}") message(STATUS "Linked ddscxx: ${DDSCXX_LIB}") diff --git a/src/MySocket.cpp b/src/MySocket.cpp index d42ea07..7bff851 100644 --- a/src/MySocket.cpp +++ b/src/MySocket.cpp @@ -62,7 +62,7 @@ bool MySocket::InitServer(int port) this->m_server_addr.sin_family = AF_INET; // 设置协议族 // this->m_server_addr.sin_addr.s_addr = INADDR_ANY; // 监听所有IP地址 - this->m_server_addr.sin_addr.s_addr = inet_addr("192.168.123.18"); // 监听指定IP地址 + this->m_server_addr.sin_addr.s_addr = inet_addr("0.0.0.0"); // 监听指定IP地址 this->m_server_addr.sin_port = htons(port); // 设置端口号 // 绑定Socket @@ -114,11 +114,6 @@ string MySocket::ReceiveData() { cout << "Client disconnected." << endl; } - // else - // { - // cout << bytes_received << endl; - // perror("recv"); - // } return ""; // 返回空字符串表示客户端断开连接 } diff --git a/src/armController.cpp b/src/armController.cpp index 0811ab7..5e648f3 100644 --- a/src/armController.cpp +++ b/src/armController.cpp @@ -22,7 +22,13 @@ void armController::Init() } // 机械臂复归原位 this->goHome(); - // fillToJntArray(this->msgData_, qLast_); +} + +// 销毁 +armController::~armController() +{ + if (arminit_ != nullptr) + delete this->arminit_; } // 从urdf构建base->tip的kdl::chain,并返回关节顺序(后续角度填写顺序) @@ -165,6 +171,108 @@ void armController::setAngle(msg_data msgData_) this->publisher_.Write(this->msg_); } +///////基本控制////////////////////////////// +void armController::goAction(const double x, const double y, const double z, const double roll, const double pitch, const double yaw) +{ + if (this->ikPoseRPY(x, y, z, roll, pitch, yaw) == false) + { + cerr << "失败" << endl; + } +} + +void armController::angleLRAction(double lr) +{ + if (abs(this->msgData_.angle0) <= 135) + this->msgData_.angle0 += lr; + else + this->msgData_.angle0 = this->msgData_.angle0 > 0 ? 135 : -135; + this->setAngle(this->msgData_); +} + +// 夹爪后退 +void armController::angleUpDownAction(double j1, double j2, double j4) +{ + if (abs(this->msgData_.angle1) <= 90) + this->msgData_.angle1 += j1; + else + this->msgData_.angle1 = this->msgData_.angle1 > 0 ? 90 : -90; + + if (abs(this->msgData_.angle2) <= 90) + this->msgData_.angle2 += j2; + else + this->msgData_.angle2 = this->msgData_.angle2 > 0 ? 90 : -90; + + if (abs(this->msgData_.angle4) <= 90) + this->msgData_.angle4 += j4; + else + this->msgData_.angle4 = this->msgData_.angle4 > 0 ? 90 : -90; + + this->setAngle(this->msgData_); +} + +void armController::angleRotateAction(double j5) +{ + if (abs(this->msgData_.angle5) <= 135) + this->msgData_.angle5 += j5; + else + this->msgData_.angle5 = this->msgData_.angle5 > 0 ? 135 : -135; + this->setAngle(this->msgData_); +} + +// 扔垃圾 +void armController::throwTrash() +{ + this->msgData_.angle0 = -0.800; + this->msgData_.angle1 = 62.500; + this->msgData_.angle2 = -30.700; + this->msgData_.angle3 = 2.099; + this->msgData_.angle4 = 44.4; + this->msgData_.angle5 = -3.099; + + this->setAngle(this->msgData_); + + std::this_thread::sleep_for(std::chrono::seconds(3)); + + this->msgData_.angle6 = 68; + this->setAngle(this->msgData_); +} + +// 夹爪向下 +void armController::goDown() +{ +} + +// 夹爪左转 +void armController::goLeft() +{ +} + +// 夹爪右转 +void armController::goRight() +{ +} + +// 夹爪放开 +void armController::jawRelease() +{ + if (this->msgData_.angle6 <= 105 && this->msgData_.angle6 >= -105) + this->msgData_.angle6 += 30; + else + this->msgData_.angle6 = this->msgData_.angle6 > 0 ? 105 : -105; + this->setAngle(this->msgData_); +} + +// 夹爪夹住 +void armController::jawClaming() +{ + if (this->msgData_.angle6 <= 105 && this->msgData_.angle6 >= -105) + this->msgData_.angle6 -= 30; + else + this->msgData_.angle6 = this->msgData_.angle6 > 0 ? 105 : -105; + this->setAngle(this->msgData_); +} + +///////预设动作////////////////////////////// // 回原点 void armController::goHome() { @@ -193,21 +301,12 @@ void armController::trashActionDefault() this->msgData_.angle3 = 2.099; this->msgData_.angle4 = 44.4; this->msgData_.angle5 = -3.099; - this->msgData_.angle6 = 68; + this->msgData_.angle6 = 66.0999984741211; this->address = 1; this->mode = 0; this->funcdoe = 2; this->setAngle(this->msgData_); - - // fillToJntArray(this->msgData_, this->qSeed_); - fillToJntArray(this->msgData_, this->qLast_); -} - -// 销毁 -armController::~armController() -{ - if (arminit_ != nullptr) - delete this->arminit_; + // fillToJntArray(this->msgData_, this->qLast_); } \ No newline at end of file diff --git a/src/armMain.cpp b/src/armMain.cpp new file mode 100644 index 0000000..b3e1287 --- /dev/null +++ b/src/armMain.cpp @@ -0,0 +1,81 @@ +#include "armcontroller.hpp" +#include +#include + +#include +#include +#include +#include + +string urdfPath = "/home/unitree/unitreeApp/urdf/d1_description/urdf/d1_description.urdf"; + +int main() +{ + armController *arm = new armController(); + + double x, y, z, row, pitch, yaw; + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + arm->trashActionDefault(); + arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); + // arm->ikPoseRPY(x, y, z, row, pitch, yaw); + // arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); + + // return 0; + std::this_thread::sleep_for(std::chrono::seconds(3)); + + arm->goHome(); + arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); + + std::this_thread::sleep_for(std::chrono::seconds(3)); + + // arm->ikPoseRPY(x, y, z, row, pitch, yaw); + + std::this_thread::sleep_for(std::chrono::seconds(5)); + + bool flag = true; + while (flag) + { + char ch; + cin >> ch; + switch (ch) + { + case 'w': + arm->goForward(); + break; + case 'a': + y += 0.05; + arm->ikPoseRPY(x, y, z, row, pitch, yaw); + break; + case 's': + x -= 0.05; + arm->ikPoseRPY(x, y, z, row, pitch, yaw); + break; + case 'd': + y -= 0.05; + arm->ikPoseRPY(x, y, z, row, pitch, yaw); + break; + case 'i': + z += 0.05; + arm->ikPoseRPY(x, y, z, row, pitch, yaw); + break; + case 'k': + z -= 0.05; + arm->ikPoseRPY(x, y, z, row, pitch, yaw); + break; + case 'c': + arm->goHome(); + arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); + break; + case 'p': + // arm->trashActionDefault(); + arm->ikPoseRPY(0.462213, -0.00559882, -0.0217322, 3.1036, 0.151366, 3.04644); + arm->getCurrentPoseFromMsg(x, y, z, row, pitch, yaw); + break; + case 'q': + flag = false; + } + } + return 0; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 0258211..7cfaed4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,13 +27,14 @@ bool ARMClientConnected = false; // 标志机械臂客户端是否连接 // 函数声明 // void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket); -void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket); // 机器人控制函数 +void Go2Control(unitree::robot::go2::SportClient sport_client, armController *arm, MySocket *Socket); // 机器人控制函数 // 视频发送函数 void SendVideo(MySocket *tcpSocket); // 狗 void SendVideoArm(MySocket *tcpSocket); // 机械臂 void Myclose(int sig); void GetControl(); void SendMergedVideo(MySocket *tcpSocket); +void SendDogVide(MySocket *tcpSocket); int main(int argc, char *argv[]) { @@ -46,12 +47,6 @@ int main(int argc, char *argv[]) signal(SIGPIPE, SIG_IGN); // 忽略 SIGPIPE 信号 signal(SIGINT, Myclose); // 捕捉 Ctrl+C 信号，调用 Myclose 函数进行清理 - // 打开串口 - // if (serial->openPort() < 0) - // { - // return -1; - // } - // 初始化 TCP 套接字服务器并等待客户端连接 Socket->InitServer(8088); // 监听端口 8088 while (true) @@ -87,11 +82,13 @@ int main(int argc, char *argv[]) sport_client.Init(); // 初始化客户端 sport_client.WaitLeaseApplied(); // 等待租约申请完成 + armController *arm = new armController(); + // 开启视频线程进行视频传输 - thread videoThread(SendMergedVideo, Socket); + thread videoThread(SendDogVide, Socket); // 进入机器人控制循环 - Go2Control(sport_client, Socket); + Go2Control(sport_client, arm, Socket); videoThread.join(); } @@ -119,12 +116,13 @@ void Myclose(int sig) } // 机器人控制逻辑 -void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket) +void Go2Control(unitree::robot::go2::SportClient sport_client, armController *arm, MySocket *Socket) { - char key = '\0'; // 当前按键 - char lastKey = '\0'; // 上一次按键 - bool isMoving = false; // 是否正在移动 - int sit = 0; // 坐下状态：1-坐下，2-趴下 + char key = '\0'; // 当前按键 + char lastKey = '\0'; // 上一次按键 + bool isMoving = false; // 是否正在移动 + int sit = 0; // 坐下状态：1-坐下，2-趴下 + bool controlStatus = true; // false:机械狗,true:机械臂 unitree::robot::ChannelFactory::Instance()->Init(0, "eth0"); // enx00e04c36141b为网口号，用户根据自身情况修改 unitree::robot::go2::ObstaclesAvoidClient sc; @@ -138,6 +136,7 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket) string recvData; // 接收的数据 sport_client.FreeAvoid(false); // 开启自由避障 + double x, y, z, roll, pitch, yaw; while (true) { @@ -148,89 +147,214 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket) cout << "DOGClient disconnected. Waiting for new connection..." << endl; break; // 如果接收数据为空，可能是客户端断开连接，退出循环 } - - // 根据接收到的指令控制机器人 - switch (static_cast(recvData[0])) + // cout << recvData << endl; + if (recvData == "CHANGE:DOG") { - case 'P': // 按 'p' 键退出 - // cout << "You pressed 'p'" << endl; - sport_client.StopMove(); // 停止运动 - return; + controlStatus = true; + cout << "Mode:" << (controlStatus ? "DOGControl" : "ARMControl") << endl; + continue; + } + else if(recvData == "CHANGE:ARM") + { + controlStatus = false; + cout << "Mode:" << (controlStatus ? "DOGControl" : "ARMControl") << endl; + continue; + } - case 'W': // 按 'w' 键向前移动 - // cout << "You pressed 'w'" << endl; - sport_client.Move(0.5f, 0, 0); - isMoving = true; - break; - - case 'S': // 按 's' 键向后移动 - // cout << "You pressed 's'" << endl; - sport_client.Move(-0.5f, 0, 0); - isMoving = true; - break; - - case 'A': // 按 'a' 键向左移动 - // cout << "You pressed 'a'" << endl; - sport_client.Move(0, 0.5f, 0); - isMoving = true; - break; - - case 'D': // 按 'd' 键向右移动 - // cout << "You pressed 'd'" << endl; - sport_client.Move(0, -0.5f, 0); - isMoving = true; - break; - - case 'Q': // 按 'q' 键左转 - // cout << "You pressed 'q'" << endl; - sport_client.Move(0, 0, 1.2f); - isMoving = true; - break; - - case 'E': // 按 'e' 键右转 - // cout << "You pressed 'e'" << endl; - sport_client.Move(0, 0, -1.2f); - isMoving = true; - break; - - case 'Z': // 按 'z' 键趴下 - // cout << "You pressed 'z'" << endl; - sport_client.StandDown(); - isMoving = false; - sit = 2; - break; - - case 'T': // 按 't' 键恢复站立 - // cout << "You pressed 't'" << endl; - if (sit == 2) + // 获取 切换字符串 + if (controlStatus == false) + { // 机械臂 + cout << "ARM:" << recvData << endl; + switch (static_cast(recvData[0])) { - sport_client.StandUp(); - sit = 0; + case 'W': + arm->angleUpDownAction(0, -5, 0); + break; + case 'S': + arm->angleUpDownAction(0, 5, 0); + break; + case 'A': + arm->angleLRAction(-5); + break; + case 'D': + arm->angleLRAction(5); + break; + case 'I': + arm->angleUpDownAction(-5, 0, 0); + break; + case 'K': + arm->angleUpDownAction(+5, 0, 0); + break; + case 'J': + arm->angleUpDownAction(0, 0, 5); + break; + case 'L': + arm->angleUpDownAction(0, 0, -5); + break; + case 'N': + arm->angleRotateAction(-5); + break; + case 'M': + arm->angleRotateAction(5); + break; + case 'P': + arm->throwTrash(); + break; + // case 'B': + // yaw += 0.05; + // break; + case 'U': + arm->jawRelease(); + break; + case 'O': + arm->jawClaming(); + break; + case 'F': + arm->trashActionDefault(); + break; + case 'C': + arm->goHome(); + break; } - else if (sit == 1) + // arm->goAction(x, y, z, roll, pitch, yaw); + // arm->getCurrentPoseFromMsg(x, y, z, roll, pitch, yaw); + } + else + { // 机械狗 + // 根据接收到的指令控制机器人 + cout << "DOG:" << recvData << endl; + switch (static_cast(recvData[0])) { - sport_client.RiseSit(); - sit = 0; - } - sport_client.StandUp(); - isMoving = false; - break; + case 'P': // 按 'p' 键退出 + // cout << "You pressed 'p'" << endl; + sport_client.StopMove(); // 停止运动 + return; - case 'F': // 按 'f' 键平衡站立 - sport_client.BalanceStand(); - break; + case 'W': // 按 'w' 键向前移动 + // cout << "You pressed 'w'" << endl; + sport_client.Move(0.5f, 0, 0); + isMoving = true; + break; - default: // 未知指令，停止运动 - if (isMoving) - { - sport_client.StopMove(); + case 'S': // 按 's' 键向后移动 + // cout << "You pressed 's'" << endl; + sport_client.Move(-0.5f, 0, 0); + isMoving = true; + break; + + case 'A': // 按 'a' 键向左移动 + // cout << "You pressed 'a'" << endl; + sport_client.Move(0, 0.5f, 0); + isMoving = true; + break; + + case 'D': // 按 'd' 键向右移动 + // cout << "You pressed 'd'" << endl; + sport_client.Move(0, -0.5f, 0); + isMoving = true; + break; + + case 'Q': // 按 'q' 键左转 + // cout << "You pressed 'q'" << endl; + sport_client.Move(0, 0, 1.2f); + isMoving = true; + break; + + case 'E': // 按 'e' 键右转 + // cout << "You pressed 'e'" << endl; + sport_client.Move(0, 0, -1.2f); + isMoving = true; + break; + + case 'Z': // 按 'z' 键趴下 + // cout << "You pressed 'z'" << endl; + sport_client.StandDown(); isMoving = false; + sit = 2; + break; + + case 'T': // 按 't' 键恢复站立 + // cout << "You pressed 't'" << endl; + if (sit == 2) + { + sport_client.StandUp(); + sit = 0; + } + else if (sit == 1) + { + sport_client.RiseSit(); + sit = 0; + } + sport_client.StandUp(); + isMoving = false; + break; + + case 'F': // 按 'f' 键平衡站立 + sport_client.BalanceStand(); + break; + + default: // 未知指令，停止运动 + if (isMoving) + { + sport_client.StopMove(); + isMoving = false; + } + break; } - break; } } } +// 仅仅发送狗的摄像头 +void SendDogVide(MySocket *tcpSocket) +{ + VideoCapture dogCap("udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1", CAP_GSTREAMER); + + if (!dogCap.isOpened()) + { + cerr << "Failed to open dog video capture." << endl; + return; + } + + dogCap.set(CAP_PROP_FPS, 30); + + Mat dogFrame; + + while (true) + { + dogCap.read(dogFrame); + + if (dogFrame.empty()) + { + continue; + } + + // 将原始图像编码为 JPEG 格式 + vector buffer; + vector compression_params = {IMWRITE_JPEG_QUALITY, 90}; // 设置 JPEG 质量，例如 90 + imencode(".jpg", dogFrame, buffer, compression_params); + + int imageSize = static_cast(buffer.size()); + + // 发送4字节大小 + if (!tcpSocket->SendData(reinterpret_cast(&imageSize), sizeof(int))) + { + cerr << "Failed to send image size." << endl; + break; + } + + // 发送图像数据 + if (!tcpSocket->SendData(reinterpret_cast(buffer.data()), buffer.size())) + { + cerr << "Failed to send image data." << endl; + continue; + } + } + + // 关闭摄像头 + dogCap.release(); +} + void SendMergedVideo(MySocket *tcpSocket) { // 使用 GStreamer 管道读取狗的视频流，加入 H.264 编码器