diff --git a/config.xml b/config/config.xml
similarity index 52%
rename from config.xml
rename to config/config.xml
index b3b6a9a..e7dbce0 100644
--- a/config.xml
+++ b/config/config.xml
@@ -1,11 +1,5 @@
-
- keyboard
- sdk
- joystick
-
-
-
+
192.168.123.110
8881
@@ -14,4 +8,5 @@
10.0
0.0
+
diff --git a/include/control/CtrlComponents.h b/include/control/CtrlComponents.h
index 6160da8..ed0524e 100755
--- a/include/control/CtrlComponents.h
+++ b/include/control/CtrlComponents.h
@@ -17,7 +17,7 @@ using namespace std;
struct CtrlComponents{
public:
- CtrlComponents();
+ CtrlComponents(int argc, char**argv);
~CtrlComponents();
std::string armConfigPath;
@@ -42,6 +42,8 @@ public:
void geneObj();
void writeData();
private:
+ void inputProcess(int argc, char** argv);
+
std::string ctrl_IP;
uint ctrl_port;
double _loadWeight;
diff --git a/include/control/joystick.h b/include/control/joystick.h
index e8b7ff1..c20cd94 100644
--- a/include/control/joystick.h
+++ b/include/control/joystick.h
@@ -2,12 +2,12 @@
#define _UNITREE_ARM_JOYSTICK_H_
#include
-#include "message/udp.h"
#include "control/cmdPanel.h"
#include "message/joystick_common.h"
#include "message/aliengo_common.h"
#include "message/b1_common.h"
#include
+#include "message/udp.h"
using namespace UNITREE_LEGGED_SDK_ALIENGO;
// using namespace UNITREE_LEGGED_SDK_B1;
@@ -18,8 +18,8 @@ public:
EmptyAction emptyAction, size_t channelNum = 1,
double dt = 0.002)
: CmdPanel(events, emptyAction, channelNum, dt){
- _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081, HIGH_STATE_LENGTH, BlockYN::NO, 500000);
-
+ _udp = new UDPPort("dog", "192.168.123.220", 8082, 8081);
+ _udp->resetIO(BlockYN::NO, HIGH_STATE_LENGTH, 500000);
_udpCmd = {0};
_udpState = {0};
_readThread = new LoopFunc("JoyStickRead", 0.0, boost::bind(&UnitreeJoystick::_read, this));
@@ -28,10 +28,9 @@ public:
_runThread->start();
};
~UnitreeJoystick(){
- delete _udp;
delete _runThread;
delete _readThread;
-
+ delete _udp;
};
private:
void _read(){
diff --git a/include/interface/IOUDP.h b/include/interface/IOUDP.h
index 992faa5..f9c267c 100644
--- a/include/interface/IOUDP.h
+++ b/include/interface/IOUDP.h
@@ -5,7 +5,7 @@
class IOUDP : public IOInterface{
public:
- IOUDP(const char* IP, uint port);
+ IOUDP(const char* IP, uint port, bool showInfo = true);
~IOUDP();
bool sendRecv(const LowlevelCmd *cmd, LowlevelState *state);
diff --git a/include/message/udp.h b/include/message/udp.h
index 6b2c2a6..e48f185 100644
--- a/include/message/udp.h
+++ b/include/message/udp.h
@@ -17,18 +17,19 @@ enum class BlockYN{
class IOPort{
public:
- IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs)
+ IOPort(std::string name, BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo)
:_name(name){
- resetIO(blockYN, recvLength, timeOutUs);
+ resetIO(blockYN, recvLength, timeOutUs, showInfo);
}
virtual ~IOPort(){}
virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0;
virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0;
virtual size_t recv(uint8_t *recvMsg) = 0;
virtual bool sendRecv(std::vector &sendVec, std::vector &recvVec) = 0;
- void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs);
+ void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs, bool showInfo = true);
bool isDisConnect = false;
protected:
+ bool _showInfo;
std::string _name;
BlockYN _blockYN = BlockYN::NO;
size_t _recvLength;
@@ -43,7 +44,8 @@ public:
UDPPort(std::string name, std::string toIP, uint toPort, uint ownPort,
size_t recvLength = 0,
BlockYN blockYN = BlockYN::NO,
- size_t timeOutUs = 20000);
+ size_t timeOutUs = 20000,
+ bool showInfo = true);
~UDPPort();
size_t send(uint8_t *sendMsg, size_t sendMsgLength);
size_t recv(uint8_t *recvMsg, size_t recvLength);
diff --git a/lib/libZ1_ROS_Linux64.so b/lib/libZ1_ROS_Linux64.so
index 9043250..6906f6f 100644
Binary files a/lib/libZ1_ROS_Linux64.so and b/lib/libZ1_ROS_Linux64.so differ
diff --git a/lib/libZ1_UDP_Linux64.so b/lib/libZ1_UDP_Linux64.so
index 4390d69..17a6bbd 100644
Binary files a/lib/libZ1_UDP_Linux64.so and b/lib/libZ1_UDP_Linux64.so differ
diff --git a/main.cpp b/main.cpp
index 2a474c0..c6066d8 100755
--- a/main.cpp
+++ b/main.cpp
@@ -17,7 +17,6 @@
#include "FSM/State_LowCmd.h"
#include "FSM/State_SetTraj.h"
-const std::string Z1_CTRL_VERSION = "2022.11.11";
bool running = true;
// over watch the ctrl+c command
@@ -32,18 +31,11 @@ void setProcessScheduler(){
sched_param param;
param.sched_priority = sched_get_priority_max(SCHED_FIFO);
if (sched_setscheduler(pid, SCHED_FIFO, ¶m) == -1) {
- std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
+ // std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl;
}
}
int main(int argc, char **argv){
- if(argc == 2) {
- if((strcmp(argv[1], "-v") == 0) || (strcmp(argv[1], "--version") == 0)){
- std::cout << "Version z1_controller: "<< Z1_CTRL_VERSION< events;
- CtrlComponents *ctrlComp = new CtrlComponents();
-
- // control method
- if(argc == 2) {
- if(argv[1][0] == 'k'){
- ctrlComp->ctrl = Control::KEYBOARD;
- }else if(argv[1][0] == 's'){
- ctrlComp->ctrl = Control::SDK;
- }else if(argv[1][0] == 'j'){
- ctrlComp->ctrl = Control::JOYSTICK;
- }
- }
+ CtrlComponents *ctrlComp = new CtrlComponents(argc, argv);
#ifdef COMPILE_WITH_SIMULATION
ros::init(argc, argv, "z1_controller");
@@ -143,7 +124,6 @@ int main(int argc, char **argv){
usleep(100000);
}
- delete fsm;
delete ctrlComp;
return 0;
}
\ No newline at end of file