Quella/unitreeApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init

Browse Source
This commit is contained in:
Quella
2025-09-26 10:32:22 +08:00
parent 7f67066355
commit 604cbccb09
7 changed files with 441 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files

81
backup/demo.cpp
View File

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

16
include/armcontroller.hpp
View File

@@ -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; // 指令地址码

26
src/CMakeLists.txt
View File

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

7
src/MySocket.cpp
View File

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

123
src/armController.cpp
View File

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

81
src/armMain.cpp Normal file
View File

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

144
src/main.cpp
View File

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