init

include/MySerial.h

@@ -0,0 +1,42 @@
+#ifndef SERIALPORT_H
+#define SERIALPORT_H
+
+#include <string>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <stdexcept>
+#include <iostream>
+#include <cstring>
+
+class SerialPort
+{
+private:
+    std::string m_portName; // 串口名称（如/dev/ttyUSB0）
+    speed_t m_baudRate;     // 串口波特率
+    int m_fd;               // 文件描述符，用于表示串口文件
+    struct termios m_tty;   // 用于保存串口配置的结构体
+public:
+    // 构造函数，初始化串口路径和波特率
+    SerialPort(const std::string &portName, speed_t baudRate);
+
+    // 析构函数，确保在对象销毁时关闭串口
+    ~SerialPort();
+
+    // 打开串口
+    int openPort();
+
+    // 关闭串口
+    void closePort();
+
+    // 写数据到串口
+    int writeData(const char *data, size_t size);
+
+    // 从串口读取数据
+    int readData(char *buffer, size_t bufferSize);
+
+    // 判断串口是否已打开
+    bool isOpen() const;
+};
+
+#endif // SERIALPORT_H

include/MySocket.h

@@ -0,0 +1,56 @@
+// 本文件主要用于接受发送端的连接,数据的收发以及处理等等功能
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <string.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+
+using namespace std;
+
+// TCP
+class MySocket
+{
+private:
+    int m_server_fd;                  // 服务端套接字文件描述符
+    int m_client_fd;                  // 客户端套接字文件描述符
+    struct sockaddr_in m_server_addr; // 服务端地址结构体
+    struct sockaddr_in m_client_addr; // 客户端地址结构体
+
+public:
+    // 构造与析构函数
+    MySocket();
+    ~MySocket();
+
+    bool InitServer(int port);              // 初始化服务端
+    bool AcceptClient();                    // 接受客户端连接
+    bool SendData(const std::string &data); // 发送数据
+    bool SendData(const char *data, size_t size); // 发送数据
+    bool SendLargeData(const string &data, size_t chunkSize);
+    string ReceiveData();     // 接收数据
+    string ReceiveLargeData(size_t expectedSize, size_t chunkSize);// 接收大数据
+    void CloseSocket();       // 关闭套接字
+    void CloseClientSocket(); // 关闭客户端套接字
+    void CloseServerSocket(); // 关闭服务端套接字
+    string GetClientIP() { return inet_ntoa(m_client_addr.sin_addr); } // 获取客户端IP地址
+    int GetClientPort() { return ntohs(m_client_addr.sin_port); }      // 获取客户端端口号
+};
+
+// UDP
+class MyUDP
+{
+private:
+    int m_sockfd;                     // UDP套接字文件描述符
+    struct sockaddr_in m_server_addr; // 对端地址结构体
+
+public:
+    MyUDP();  // 构造函数
+    ~MyUDP(); // 析构函数
+
+    bool InitServer(const string &ip, int port); // 初始化UDP服务端
+    bool SendData(const std::string &data);      // 发送数据
+    string ReceiveData();                        // 接收数据
+    void CloseSocket();                          // 关闭套接字
+};

include/armcontroller.hpp

@@ -0,0 +1,124 @@
+/*
+本程序用于机械臂的基本控制
+*/
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <string>
+#include "msg/ArmString_.hpp"
+
+#include <urdf_model/model.h>                 // 解析 URDF文本/文件
+#include <kdl_parser/kdl_parser.hpp>          //URDF -> KDL::Tree
+#include <kdl/tree.hpp>                       //结构树
+#include <kdl/chain.hpp>                      //让solver可识别
+#include <kdl/jntarray.hpp>                   //数据容器
+#include <kdl/frames.hpp>                     //位姿工具提供
+#include <kdl/chainfksolverpos_recursive.hpp> //前向运动学
+#include <kdl/chainiksolverpos_lma.hpp>       //逆向运动学
+
+#define TOPIC "rt/arm_Command"
+#define TOPICSTATUS "current_servo_angle"
+#define TOPICSTATUS1 "rt/arm_Feedback"
+
+using namespace std;
+using namespace unitree::robot;
+using namespace unitree::common;
+
+// 弧度互转工具,角度转弧度
+static inline double rad2deg(double r) { return r * 180.0 / M_PI; }
+static inline double deg2rad(double d) { return d * M_PI / 180.0; }
+
+// 消息结构体
+struct msg_data
+{
+    // 电机编号
+    double angle0; // 底座
+    double angle1;
+    double angle2;
+    double angle3;
+    double angle4;
+    double angle5;
+    double angle6; // 夹爪
+};
+
+// struct anglePos
+// {
+//     double x;
+//     double y;
+//     double z;
+//     double roll;
+//     double pitch;
+//     double yaw;
+// };
+
+// 机械臂控制对象
+class armController
+{
+public:
+    armController();
+    ~armController();
+    void Init();
+
+    // 从urdf构建base->tip的kdl::chain,并返回关节顺序(后续角度填写顺序)
+    bool buildChainFromUrdf(const string &urdfPath, const string &baseLink, const string &tipLink, KDL::Chain &outChain, vector<string> &outJointNames);
+    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);
+
+    // 填充至JntArray
+    void fillToJntArray(const msg_data &msgdata_, KDL::JntArray &qSol);
+
+    // 填充至结构体中
+    void fillToMsgData(const KDL::JntArray &qSol, msg_data &msgdata);
+
+    ///////基本控制///////
+    // 夹爪前进
+    void goForward();
+
+    // 夹爪后退
+    void goBack();
+
+    // 夹爪向上
+    void goUp();
+
+    // 夹爪向下
+    void goDown();
+
+    // 夹爪左转
+    void goLeft();
+
+    // 夹爪右转
+    void goRight();
+
+    // 夹爪放开
+    void jawRelease();
+
+    // 夹爪夹住
+    void jawClaming();
+
+    ///////预设位置///////
+    // 回原点
+    void goHome();
+
+    // 捡垃圾预设位置
+    void trashActionDefault();
+
+    // 设置电机参数
+    void setAngle(msg_data msgData_);
+
+private:
+    // 发送消息
+    msg_data msgData_{};
+    uint seq = 0;                                                           // 唯一识别码
+    uint funcdoe = 0;                                                       // 指令功能码
+    uint address = 0;                                                       // 指令地址码
+    uint mode = 0;                                                          // 控制模式
+    unitree_arm::msg::dds_::ArmString_ msg_{};                              // 传送消息
+    unitree::robot::ChannelFactory *arminit_ = ChannelFactory::Instance();  // 初始化句柄
+    ChannelPublisher<unitree_arm::msg::dds_::ArmString_> publisher_{TOPIC}; // 机械臂dds控制句柄
+    KDL::Chain outchain_;                                                   // 保存机械臂结构
+    KDL::JntArray qSeed_;                                                   // 当前位姿
+    KDL::JntArray qLast_;                                                   // 保存上次的位姿
+    KDL::JntArray qSol_;                                                    // 输出位姿
+    vector<string> outJoinName_;                                            // 保存关节顺序
+};