init

backup/demo.cpp

@@ -1,81 +0,0 @@
-// #include "armcontroller.hpp"
-// #include <iostream>
-// #include <thread>
-
-// #include <termios.h>
-// #include <unistd.h>
-// #include <sys/select.h>
-// #include <cstdio>
-
-// 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;
-// }

include/armcontroller.hpp

@@ -14,6 +14,7 @@
 #include <kdl/frames.hpp>                     //位姿工具提供
 #include <kdl/chainfksolverpos_recursive.hpp> //前向运动学
 #include <kdl/chainiksolverpos_lma.hpp>       //逆向运动学
+#include <thread>
 
 #define TOPIC "rt/arm_Command"
 #define TOPICSTATUS "current_servo_angle"
@@ -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;                                                       // 指令地址码

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}")

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 ""; // 返回空字符串表示客户端断开连接
     }
 

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->qLast_);
-    // fillToJntArray(this->msgData_, this->qSeed_);
-    fillToJntArray(this->msgData_, this->qLast_);
-}
-
-// 销毁
-armController::~armController()
-{
-    if (arminit_ != nullptr)
-        delete this->arminit_;
 }

src/armMain.cpp

@@ -0,0 +1,81 @@
+#include "armcontroller.hpp"
+#include <iostream>
+#include <thread>
+
+#include <termios.h>
+#include <unistd.h>
+#include <sys/select.h>
+#include <cstdio>
+
+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;
+}

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-趴下
+    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,8 +147,82 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket)
             cout << "DOGClient disconnected. Waiting for new connection..." << endl;
             break; // 如果接收数据为空，可能是客户端断开连接，退出循环
         }
+        // cout << recvData << endl;
+        if (recvData == "CHANGE:DOG")
+        {
+            controlStatus = true;
+            cout << "Mode:" << (controlStatus ? "DOGControl" : "ARMControl") << endl;
+            continue;
+        }
+        else if(recvData == "CHANGE:ARM")
+        {
+            controlStatus = false;
+            cout << "Mode:" << (controlStatus ? "DOGControl" : "ARMControl") << endl;
+            continue;
+        }
 
+        // 获取 切换字符串
+        if (controlStatus == false)
+        { // 机械臂
+            cout << "ARM:" << recvData << endl;
+            switch (static_cast<char>(recvData[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;
+            }
+            // arm->goAction(x, y, z, roll, pitch, yaw);
+            // arm->getCurrentPoseFromMsg(x, y, z, roll, pitch, yaw);
+        }
+        else
+        { // 机械狗
           //  根据接收到的指令控制机器人
+            cout << "DOG:" << recvData << endl;
             switch (static_cast<char>(recvData[0]))
             {
             case 'P': // 按 'p' 键退出
@@ -230,6 +303,57 @@ void Go2Control(unitree::robot::go2::SportClient sport_client, MySocket *Socket)
             }
         }
     }
+}
+
+// 仅仅发送狗的摄像头
+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<uchar> buffer;
+        vector<int> compression_params = {IMWRITE_JPEG_QUALITY, 90}; // 设置 JPEG 质量，例如 90
+        imencode(".jpg", dogFrame, buffer, compression_params);
+
+        int imageSize = static_cast<int>(buffer.size());
+
+        // 发送4字节大小
+        if (!tcpSocket->SendData(reinterpret_cast<const char *>(&imageSize), sizeof(int)))
+        {
+            cerr << "Failed to send image size." << endl;
+            break;
+        }
+
+        // 发送图像数据
+        if (!tcpSocket->SendData(reinterpret_cast<const char *>(buffer.data()), buffer.size()))
+        {
+            cerr << "Failed to send image data." << endl;
+            continue;
+        }
+    }
+
+    // 关闭摄像头
+    dogCap.release();
+}
 
 void SendMergedVideo(MySocket *tcpSocket)
 {