diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..b75c00037ba1f488cab394476e98a8431268d4b0 --- /dev/null +++ b/.gitignore @@ -0,0 +1,42 @@ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +*.lib + +# Executables +*.exe +*.out +*.app + +# build path +.vscode +*.idea +*node_modules +*.hvigor +*.cxx +*build +*.clang* +*.preview \ No newline at end of file diff --git a/dsoftbus/openEuler_softbus_client/include/softbus_init.h b/dsoftbus/openEuler_softbus_client/include/softbus_init.h new file mode 100644 index 0000000000000000000000000000000000000000..a5513606a9896894beec672e6e790d2c88b4f735 --- /dev/null +++ b/dsoftbus/openEuler_softbus_client/include/softbus_init.h @@ -0,0 +1,70 @@ +#include +#include +#include "securec.h" +#include "discovery_service.h" +#include "softbus_bus_center.h" +#include "session.h" + +#define UN_OPENED_SESSIONID -1 + +struct SoftClientInitParam +{ + const char *package_name; + const char *local_session_name; + const char *target_session_name; + const char *default_capability; + const char *default_session_group; + int default_publish_id; + int opened_sessionId; +}; + +void logger(const char *__restrict __fmt, ...); + +void DefaultPublishSuccess(int publishId); + +void DefaultPublishFailed(int publishId, PublishFailReason reason); + +void UnPublishServiceInterface(SoftClientInitParam *param); + +void DeviceFound(const DeviceInfo *device); + +void DiscoverySuccess(int subscribeId); + +void DiscoveryFailed(int subscribeId, DiscoveryFailReason reason); + +void StopDiscoveryInterface(SoftClientInitParam *param); + +void SessionClosed(int sessionId); + +void ByteRecived(int sessionId, const void *data, unsigned int dataLen); + +void MessageReceived(int sessionId, const void *data, unsigned int dataLen); + +void RemoveSessionServerInterface(SoftClientInitParam *param); + +int OpenSessionInterface(SoftClientInitParam *param, const char *peerNetworkId); + +void CloseSessionInterface(int sessionId); + +int GetAllNodeDeviceInfoInterface(SoftClientInitParam *param, NodeBasicInfo **dev); + +void FreeNodeInfoInterface(NodeBasicInfo *dev); + +void commnunicate(SoftClientInitParam *param); + +int testSoftbus(SoftClientInitParam *param, int (*SessionOpened)(int sessionId, int result)); + +int PublishServiceInterface(SoftClientInitParam *param, + void (*PublishSuccess)(int publishId) = DefaultPublishSuccess, + void (*PublishFailed)(int publishId, PublishFailReason reason) = DefaultPublishFailed); + +int DiscoveryInterface(SoftClientInitParam *param, + void (*OnDeviceFound)(const DeviceInfo *device) = DeviceFound, + void (*OnDiscoverFailed)(int subscribeId, DiscoveryFailReason failReason) = DiscoveryFailed, + void (*OnDiscoverySuccess)(int subscribeId) = DiscoverySuccess); + +int CreateSessionServerInterface(SoftClientInitParam *param, + int (*SessionOpened)(int sessionId, int result), + void (*SessionClosed)(int sessionId) = SessionClosed, + void (*ByteRecived)(int sessionId, const void *data, unsigned int dataLen) = ByteRecived, + void (*MessageReceived)(int sessionId, const void *data, unsigned int dataLen) = MessageReceived); diff --git a/dsoftbus/openEuler_softbus_client/openEuler_softbus_client.c b/dsoftbus/openEuler_softbus_client/openEuler_softbus_client.c deleted file mode 100644 index cdbaf55f5b467225c0f44ff13359605c593c8311..0000000000000000000000000000000000000000 --- a/dsoftbus/openEuler_softbus_client/openEuler_softbus_client.c +++ /dev/null @@ -1,301 +0,0 @@ -#include -#include -#include -#include "securec.h" -#include "discovery_service.h" -#include "softbus_bus_center.h" -#include "session.h" - -#define PACKAGE_NAME "softbus_sample" -#define LOCAL_SESSION_NAME "session_test" -#define TARGET_SESSION_NAME "session_test" -#define DEFAULT_CAPABILITY "osdCapability" -#define DEFAULT_SESSION_GROUP "group_test" -#define DEFAULT_PUBLISH_ID 123 - -static int g_sessionId; - -static void PublishSuccess(int publishId) -{ - printf("CB: publish %d done\n", publishId); -} - -static void PublishFailed(int publishId, PublishFailReason reason) -{ - printf("CB: publish %d failed, reason=%d\n", publishId, (int)reason); -} - -static int PublishServiceInterface() -{ - PublishInfo info = { - .publishId = DEFAULT_PUBLISH_ID, - .mode = DISCOVER_MODE_PASSIVE, - .medium = COAP, - .freq = LOW, - .capability = DEFAULT_CAPABILITY, - .capabilityData = NULL, - .dataLen = 0, - }; - IPublishCallback cb = { - .OnPublishSuccess = PublishSuccess, - .OnPublishFail = PublishFailed, - }; - return PublishService(PACKAGE_NAME, &info, &cb); -} - -static void UnPublishServiceInterface(void) -{ - int ret = UnPublishService(PACKAGE_NAME, DEFAULT_PUBLISH_ID); - if (ret != 0) { - printf("UnPublishService fail:%d\n", ret); - } -} - -static void DeviceFound(const DeviceInfo *device) -{ - unsigned int i; - printf("CB: Device has found\n"); - printf("\tdevId=%s\n", device->devId); - printf("\tdevName=%s\n", device->devName); - printf("\tdevType=%d\n", device->devType); - printf("\taddrNum=%d\n", device->addrNum); - for (i = 0; i < device->addrNum; i++) { - printf("\t\taddr%d:type=%d,", i + 1, device->addr[i].type); - switch (device->addr[i].type) { - case CONNECTION_ADDR_WLAN: - case CONNECTION_ADDR_ETH: - printf("ip=%s,port=%d,", device->addr[i].info.ip.ip, device->addr[i].info.ip.port); - break; - default: - break; - } - printf("peerUid=%s\n", device->addr[i].peerUid); - } - printf("\tcapabilityBitmapNum=%d\n", device->capabilityBitmapNum); - for (i = 0; i < device->addrNum; i++) { - printf("\t\tcapabilityBitmap[%d]=0x%x\n", i + 1, device->capabilityBitmap[i]); - } - printf("\tcustData=%s\n", device->custData); -} - -static void DiscoverySuccess(int subscribeId) -{ - printf("CB: discover subscribeId=%d\n", subscribeId); -} - -static void DiscoveryFailed(int subscribeId, DiscoveryFailReason reason) -{ - printf("CB: discover subscribeId=%d failed, reason=%d\n", subscribeId, (int)reason); -} - -static int DiscoveryInterface(void) -{ - SubscribeInfo info = { - .subscribeId = DEFAULT_PUBLISH_ID, - .mode = DISCOVER_MODE_ACTIVE, - .medium = COAP, - .freq = LOW, - .isSameAccount = false, - .isWakeRemote = false, - .capability = DEFAULT_CAPABILITY, - .capabilityData = NULL, - .dataLen = 0, - }; - IDiscoveryCallback cb = { - .OnDeviceFound = DeviceFound, - .OnDiscoverFailed = DiscoveryFailed, - .OnDiscoverySuccess = DiscoverySuccess, - }; - return StartDiscovery(PACKAGE_NAME, &info, &cb); -} - -static void StopDiscoveryInterface(void) -{ - int ret = StopDiscovery(PACKAGE_NAME, DEFAULT_PUBLISH_ID); - if (ret) { - printf("StopDiscovery fail:%d\n", ret); - } -} - -static int SessionOpened(int sessionId, int result) -{ - printf("CB: session %d open fail:%d\n", sessionId, result); - if (result == 0) { - g_sessionId = sessionId; - } - - return result; -} - -static void SessionClosed(int sessionId) -{ - printf("CB: session %d closed\n", sessionId); -} - -static void ByteRecived(int sessionId, const void *data, unsigned int dataLen) -{ - printf("CB: session %d received %u bytes data=%s\n", sessionId, dataLen, (const char *)data); -} - -static void MessageReceived(int sessionId, const void *data, unsigned int dataLen) -{ - printf("CB: session %d received %u bytes message=%s\n", sessionId, dataLen, (const char *)data); -} - -static int CreateSessionServerInterface(void) -{ - const ISessionListener sessionCB = { - .OnSessionOpened = SessionOpened, - .OnSessionClosed = SessionClosed, - .OnBytesReceived = ByteRecived, - .OnMessageReceived = MessageReceived, - }; - - return CreateSessionServer(PACKAGE_NAME, LOCAL_SESSION_NAME, &sessionCB); -} - -static void RemoveSessionServerInterface(void) -{ - int ret = RemoveSessionServer(PACKAGE_NAME, LOCAL_SESSION_NAME); - if (ret) { - printf("RemoveSessionServer fail:%d\n", ret); - } -} - -static int OpenSessionInterface(const char *peerNetworkId) -{ - SessionAttribute attr = { - .dataType = TYPE_BYTES, - .linkTypeNum = 1, - .linkType[0] = LINK_TYPE_WIFI_WLAN_2G, - .attr = {RAW_STREAM}, - }; - - return OpenSession(LOCAL_SESSION_NAME, TARGET_SESSION_NAME, peerNetworkId, DEFAULT_SESSION_GROUP, &attr); -} - -static void CloseSessionInterface(int sessionId) -{ - CloseSession(sessionId); -} - -static int GetAllNodeDeviceInfoInterface(NodeBasicInfo **dev) -{ - int ret, num; - - ret = GetAllNodeDeviceInfo(PACKAGE_NAME, dev, &num); - if (ret) { - printf("GetAllNodeDeviceInfo fail:%d\n", ret); - return -1; - } - - printf("return %d Node\n", num); - return num; -} - -static void FreeNodeInfoInterface(NodeBasicInfo *dev) -{ - FreeNodeInfo(dev); -} - -static void commnunicate(void) -{ - NodeBasicInfo *dev = NULL; - char cData[] = "hello world test"; - int dev_num, sessionId, input, ret; - int timeout = 5; - - dev_num = GetAllNodeDeviceInfoInterface(&dev); - if (dev_num <= 0) { - return; - } - - for (int i = 0; i < dev_num; i++) { - char devId[UDID_BUF_LEN]; - printf("deviceName=%s\n", i + 1, dev[i].deviceName); - printf("\tnetworkId=%s\n", dev[i].networkId); - if (GetNodeKeyInfo(PACKAGE_NAME, dev[i].networkId, NODE_KEY_UDID, (uint8_t *)devId, UDID_BUF_LEN) == 0) { - printf("\tdevId=%s\n", devId); - } - printf("\tType=%d\n", dev[i].deviceTypeId); - } - - printf("\nInput Node num to commnunication:"); - scanf_s("%d", &input); - if (input <= 0 || input > dev_num) { - printf("error input num\n"); - goto err_input; - } - - g_sessionId = -1; - sessionId = OpenSessionInterface(dev[input - 1].networkId); - if (sessionId < 0) { - printf("OpenSessionInterface fail, ret=%d\n", sessionId); - goto err_OpenSessionInterface; - } - - while (timeout) { - if (g_sessionId == sessionId) { - ret = SendBytes(sessionId, cData, strlen(cData) + 1); - if (ret) { - printf("SendBytes fail:%d\n", ret); - } - break; - } - timeout--; - sleep(1); - } - - CloseSessionInterface(sessionId); -err_OpenSessionInterface: -err_input: - FreeNodeInfoInterface(dev); -} - -int main(int argc, char **argv) -{ - bool loop = true; - int ret; - - ret = CreateSessionServerInterface(); - if (ret) { - printf("CreateSessionServer fail, ret=%d\n", ret); - return ret; - } - - ret = PublishServiceInterface(); - if (ret) { - printf("PublishService fail, ret=%d\n", ret); - goto err_PublishServiceInterface; - } - - ret = DiscoveryInterface(); - if (ret) { - printf("DiscoveryInterface fail, ret=%d\n", ret); - goto err_DiscoveryInterface; - } - - while (loop) { - printf("\nInput c to commnuication, Input s to stop:"); - char op = getchar(); - switch(op) { - case 'c': - commnunicate(); - continue; - case 's': - loop = false; - break; - case '\n': - break; - default: - continue; - } - } - - StopDiscoveryInterface(); -err_DiscoveryInterface: - UnPublishServiceInterface(); -err_PublishServiceInterface: - RemoveSessionServerInterface(); - return 0; -} diff --git a/dsoftbus/openEuler_softbus_client/src/softbus_init_imp.cpp b/dsoftbus/openEuler_softbus_client/src/softbus_init_imp.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a30c3232443845f35e5dce538c02747d2ed7a341 --- /dev/null +++ b/dsoftbus/openEuler_softbus_client/src/softbus_init_imp.cpp @@ -0,0 +1,296 @@ +#include +#include +#include "softbus_init.h" + + +void logger(const char *__restrict __fmt, ...) { + printf(__fmt); +} + +void DefaultPublishSuccess(int publishId) +{ + logger("CB: publish %d done", publishId); +} + +void DefaultPublishFailed(int publishId, PublishFailReason reason) +{ + logger("CB: publish %d failed, reason= %d \n", publishId, (int)reason); +} + +void UnPublishServiceInterface(SoftClientInitParam *param) +{ + int ret = UnPublishService(param->package_name, param->default_publish_id); + if (ret != 0) { + logger("UnPublishService fail:%d \n ", ret); + } +} + +void DeviceFound(const DeviceInfo *device) +{ + unsigned int i; + logger("CB: Device has found\n"); + logger("\tdevId=%s\n", device->devId); + logger("\tdevName=%s\n", device->devName); + logger("\tdevType=%d\n", device->devType); + logger("\taddrNum=%d\n", device->addrNum); + for (i = 0; i < device->addrNum; i++) { + logger("\t\taddr%d:type=%d,", i + 1, device->addr[i].type); + switch (device->addr[i].type) { + case CONNECTION_ADDR_WLAN: + case CONNECTION_ADDR_ETH: + logger("ip=%s,port=%d,", device->addr[i].info.ip.ip, device->addr[i].info.ip.port); + break; + default: + break; + } + logger("peerUid=%s\n", device->addr[i].peerUid); + } + logger("\tcapabilityBitmapNum=%d\n", device->capabilityBitmapNum); + for (i = 0; i < device->addrNum; i++) { + logger("\t\tcapabilityBitmap[%d]=0x%x\n", i + 1, device->capabilityBitmap[i]); + } + logger("\tcustData=%s\n", device->custData); +} + +void DiscoverySuccess(int subscribeId) +{ + logger("CB: discover subscribeId=%d\n", subscribeId); +} + +void DiscoveryFailed(int subscribeId, DiscoveryFailReason reason) +{ + logger("CB: discover subscribeId=%d failed, reason=%d\n", subscribeId, (int)reason); +} + +void StopDiscoveryInterface(SoftClientInitParam *param) +{ + int ret = StopDiscovery(param->package_name, param->default_publish_id); + if (ret) { + logger("StopDiscovery fail:%d\n", ret); + } +} + +void SessionClosed(int sessionId) +{ + logger("CB: session %d closed\n", sessionId); +} + +void ByteRecived(int sessionId, const void *data, unsigned int dataLen) +{ + const char *inputStr = (const char *)data; + logger("CB: session %d received %u bytes data=%s\n", sessionId, dataLen, inputStr); +} + +void MessageReceived(int sessionId, const void *data, unsigned int dataLen) +{ + logger("CB: session %d received %u bytes message=%s\n", sessionId, dataLen, (const char *)data); +} + +void RemoveSessionServerInterface(SoftClientInitParam *param) +{ + int ret = RemoveSessionServer(param->package_name, param->local_session_name); + if (ret) { + logger("RemoveSessionServer fail:%d\n", ret); + } +} + +int OpenSessionInterface(SoftClientInitParam *param, const char *peerNetworkId) +{ + SessionAttribute attr = { + .dataType = TYPE_BYTES, + .linkTypeNum = 1, + .attr = {RAW_STREAM}, + }; + attr.linkType[0] = LINK_TYPE_WIFI_WLAN_2G; + return OpenSession(param->local_session_name, param->target_session_name, peerNetworkId, param->default_session_group, &attr); +} + +void CloseSessionInterface(int sessionId) +{ + CloseSession(sessionId); +} + +int GetAllNodeDeviceInfoInterface(SoftClientInitParam *param, NodeBasicInfo **dev) +{ + int ret, num; + + ret = GetAllNodeDeviceInfo(param->package_name, dev, &num); + if (ret) { + logger("GetAllNodeDeviceInfo fail:%d\n", ret); + return -1; + } + + logger("return %d Node\n", num); + return num; +} + +void FreeNodeInfoInterface(NodeBasicInfo *dev) +{ + FreeNodeInfo(dev); +} + +void commnunicate(SoftClientInitParam *param) +{ + NodeBasicInfo *dev = NULL; + char cData[] = "hello world test"; + int dev_num, sessionId, input, ret; + int timeout = 5; + + dev_num = GetAllNodeDeviceInfoInterface(param, &dev); + if (dev_num <= 0) { + logger("no device online \n"); + return; + } + + for (int i = 0; i < dev_num; i++) { + char devId[UDID_BUF_LEN]; + logger("deviceName=%s\n", i + 1, dev[i].deviceName); + logger("\tnetworkId=%s\n", dev[i].networkId); + if (GetNodeKeyInfo(param->package_name, dev[i].networkId, NODE_KEY_UDID, (uint8_t *)devId, UDID_BUF_LEN) == 0) { + logger("\tdevId=%s\n", devId); + } + logger("\tType=%d\n", dev[i].deviceTypeId); + } + + logger("\nInput Node num to commnunication:"); + scanf_s("%d", &input); + if (input <= 0 || input > dev_num) { + logger("error input num\n"); + goto err_input; + } + + param->opened_sessionId = -1; + sessionId = OpenSessionInterface(param, dev[input - 1].networkId); + if (sessionId < 0) { + logger("OpenSessionInterface fail, ret=%d\n", sessionId); + goto err_OpenSessionInterface; + } + + while (timeout) { + if (param->opened_sessionId == sessionId) { + logger("\nInput send bytes\n"); + scanf_s("%s", cData); + ret = SendBytes(sessionId, cData, strlen(cData) + 1); + if (ret) { + logger("SendBytes fail:%d\n", ret); + } + break; + } + timeout--; + sleep(1); + } + + CloseSessionInterface(sessionId); +err_OpenSessionInterface: +err_input: + FreeNodeInfoInterface(dev); +} + +int PublishServiceInterface(SoftClientInitParam *param, + void (*PublishSuccess)(int publishId) /*= DefaultPublishSuccess*/, + void (*PublishFailed)(int publishId, PublishFailReason reason) /* = DefaultPublishFailed */) +{ + PublishInfo info = { + .publishId = param->default_publish_id, + .mode = DISCOVER_MODE_PASSIVE, + .medium = COAP, + .freq = LOW, + .capability = param->default_capability, + .capabilityData = NULL, + .dataLen = 0, + }; + IPublishCallback cb = { + .OnPublishSuccess = PublishSuccess, + .OnPublishFail = PublishFailed, + }; + return PublishService(param->package_name, &info, &cb); +} + +int DiscoveryInterface(SoftClientInitParam *param, + void (*OnDeviceFound)(const DeviceInfo *device) /* = DeviceFound */, + void (*OnDiscoverFailed)(int subscribeId, DiscoveryFailReason failReason) /* = DiscoveryFailed */, + void (*OnDiscoverySuccess)(int subscribeId) /* = DiscoverySuccess */) +{ + SubscribeInfo info = { + .subscribeId = param->default_publish_id, + .mode = DISCOVER_MODE_ACTIVE, + .medium = COAP, + .freq = LOW, + .isSameAccount = false, + .isWakeRemote = false, + .capability = param->default_capability, + .capabilityData = NULL, + .dataLen = 0, + }; + IDiscoveryCallback cb = { + .OnDeviceFound = OnDeviceFound, + .OnDiscoverFailed = OnDiscoverFailed, + .OnDiscoverySuccess = OnDiscoverySuccess, + }; + return StartDiscovery(param->package_name, &info, &cb); +} + +int CreateSessionServerInterface(SoftClientInitParam *param, + int (*SessionOpened)(int sessionId, int result), + void (*SessionClosed)(int sessionId) /* = SessionClosed */, + void (*ByteRecived)(int sessionId, const void *data, unsigned int dataLen) /* = ByteRecived */, + void (*MessageReceived)(int sessionId, const void *data, unsigned int dataLen) /* = MessageReceived */) +{ + const ISessionListener sessionCB = { + .OnSessionOpened = SessionOpened, + .OnSessionClosed = SessionClosed, + .OnBytesReceived = ByteRecived, + .OnMessageReceived = MessageReceived, + }; + + return CreateSessionServer(param->package_name, param->local_session_name, &sessionCB); +} + +int testSoftbus(SoftClientInitParam *param, int (*SessionOpened)(int sessionId, int result)) +{ + bool loop = true; + int ret; + + ret = CreateSessionServerInterface(param, SessionOpened); + if (ret) { + logger("CreateSessionServer fail, ret=%d\n", ret); + return ret; + } + + ret = PublishServiceInterface(param); + if (ret) { + logger("PublishService fail, ret=%d\n", ret); + goto err_PublishServiceInterface; + } + + ret = DiscoveryInterface(param); + if (ret) { + logger("DiscoveryInterface fail, ret=%d\n", ret); + goto err_DiscoveryInterface; + } + + while (loop) { + logger("\nInput c to commnuication, Input s to stop:"); + char op = getchar(); + switch(op) { + case 'c': + commnunicate(param); + continue; + case 's': + loop = false; + break; + case '\n': + break; + default: + continue; + } + } + + StopDiscoveryInterface(param); +err_DiscoveryInterface: + UnPublishServiceInterface(param); +err_PublishServiceInterface: + RemoveSessionServerInterface(param); + return 0; +} + diff --git a/dsoftbus/unitree_softbus_demo/.gitignore b/dsoftbus/unitree_softbus_demo/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..05a6dd23c1e8694255e035a1ddd478851841ba18 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/.gitignore @@ -0,0 +1,3 @@ +/lcm-1.4.0 +/build +/.vscode diff --git a/dsoftbus/unitree_softbus_demo/CMakeLists.txt b/dsoftbus/unitree_softbus_demo/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..f4e69a92b5ba457960a12c3106f5f9af77014903 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 2.8.3) +project(unitree_legged_sdk) + +include_directories(include ../OhOeCommunication/OpenHarmonyAppSample/entry/src/main/cpp/include ../openEuler_softbus_client/include) + +link_directories(lib /usr/lib64) + +add_compile_options(-std=c++11) + +if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "x86_64" ) + set(EXTRA_SOFTBUS_LIBS -pthread libunitree_legged_sdk_amd64.so lcm softbus_client.z boundscheck) +else() + set(EXTRA_SOFTBUS_LIBS -pthread libunitree_legged_sdk_arm64.so lcm softbus_client.z boundscheck) +endif() + +set(CMAKE_CXX_FLAGS "-O3") + +#softbus_client +add_executable(robot_client softbus_demo/client/robot_walk.cpp ../openEuler_softbus_client/src/softbus_init_imp.cpp softbus_demo/client/robot_client.cpp) +target_link_libraries(robot_client ${EXTRA_SOFTBUS_LIBS}) diff --git a/dsoftbus/unitree_softbus_demo/README.md b/dsoftbus/unitree_softbus_demo/README.md new file mode 100644 index 0000000000000000000000000000000000000000..03e0584c1287be0c3cd38acd12d4970a4cb0e075 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/README.md @@ -0,0 +1,54 @@ +# v3.3.1 +The unitree_legged_sdk is mainly used for communication between PC and Controller board. +It also can be used in other PCs with UDP. + +## Notice +support robot: Aliengo, A1(sport_mode >= v1.19) +support OS: openEuler-22.03-LTS-SP1 + +**not support robot**: Laikago, Go1. + +## Dependencies +* [Boost](http://www.boost.org) (version 1.5.4 or higher) +* [CMake](http://www.cmake.org) (version 2.8.3 or higher) +* [LCM](https://lcm-proj.github.io) (version 1.4.0 or higher) + +## Prepare + +### Install dependecies +```bash +dnf install glib2-devel cmake make gcc g++ boost boost-devel dsoftbus libboundscheck +``` + +### Build LCM +```bash +cd lcm-x.x.x +mkdir build +cd build +cmake ../ +make +sudo make install +``` + +## Build Demo +```bash +mkdir build +cd build +cmake ../ +make +``` + +## Usage +Run examples with 'sudo' for memory locking. +```bash +sudo ./robot_client +``` + +error in openEuler +- error while loading shared libraries: liblcm.so.1: cannot open shared object file: No such file or directory +```bash +sudo ln -s /usr/local/lib/liblcm.so.1 /lib64/liblcm.so.1 +``` + +## More about the softbus +https://docs.openeuler.org/zh/docs/22.03_LTS_SP1/docs/Distributed/%E5%9F%BA%E4%BA%8E%E5%88%86%E5%B8%83%E5%BC%8F%E8%BD%AF%E6%80%BB%E7%BA%BF%E6%89%A9%E5%B1%95%E7%94%9F%E6%80%81%E4%BA%92%E8%81%94.html diff --git a/dsoftbus/unitree_softbus_demo/include/softbus_sdk/robot_walk.h b/dsoftbus/unitree_softbus_demo/include/softbus_sdk/robot_walk.h new file mode 100644 index 0000000000000000000000000000000000000000..af19904894b477128820c9ec7436a5c234ca711d --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/softbus_sdk/robot_walk.h @@ -0,0 +1,70 @@ +#include "unitree_legged_sdk/unitree_legged_sdk.h" +#include +#include +#include +#include +#include +#include + +using namespace UNITREE_LEGGED_SDK; + +#define ALL_TEST 0 +#define WALK_TEST 1 +#define WALK_YAW_LEFT 7 +#define WALK_YAW_RIGHT 9 +#define WALK_FORWARD 8 +#define WALK_FORWARD 8 +#define WALK_LEFT 4 +#define WALK_RIGHT 6 +#define WALK_BACKWARD 2 +#define STAND_UP 0 +#define SIT_DOWN 1 + +class Custom +{ +public: + Custom(uint8_t level) : safe(LeggedType::A1), udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)) + { + udp.InitCmdData(cmd); + logfile.open("dog.log"); + } + void UDPRecv(); + void UDPSend(); + void RobotControl(); + void RobotForward(); + void RobotLeft(); + void RobotRight(); + void RobotBackward(); + void RobotYawLeft(); + void RobotYawRight(); + void RobotStandUp(); + void RobotSitDown(); + void dogStartTest(); + void walking(); + void dogDoActionWithSoftbus(string key); + void log(const char *__restrict __format, ...); + + Safety safe; + UDP udp; + HighCmd cmd = {0}; + HighState state = {0}; + int motiontime = 0; + float dt = 0.002; // 0.001~0.01 + +private: + void initStandCmd(); + void dogDoAction(int type); + + ofstream logfile; + std::map actionMap = { + {"forward", 8}, + {"backward", 2}, + {"left", 4}, + {"right", 6}, + {"clockwise", 7}, + {"counterclockwise", 9}, + {"standup", 0}, + {"sitdown", 1} + }; + +}; diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/a1_const.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/a1_const.h new file mode 100644 index 0000000000000000000000000000000000000000..84e8436c8fdb4ef7187b7e70cabf93de7dabfb17 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/a1_const.h @@ -0,0 +1,18 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_A1_H_ +#define _UNITREE_LEGGED_A1_H_ + +namespace UNITREE_LEGGED_SDK +{ + constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) + constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) + constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) + constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) + constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) + constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/aliengo_const.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/aliengo_const.h new file mode 100644 index 0000000000000000000000000000000000000000..12c3138cc9254b3ca021e79f21b908f74e049b8d --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/aliengo_const.h @@ -0,0 +1,18 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_ALIENGO_H_ +#define _UNITREE_LEGGED_ALIENGO_H_ + +namespace UNITREE_LEGGED_SDK +{ + constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) + constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) + constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) + constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) + constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) + constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/comm.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/comm.h new file mode 100644 index 0000000000000000000000000000000000000000..eef3064e983879d6b8a7022caef9cee208dbe837 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/comm.h @@ -0,0 +1,177 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_COMM_H_ +#define _UNITREE_LEGGED_COMM_H_ + +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr int HIGHLEVEL = 0x00; + constexpr int LOWLEVEL = 0xff; + constexpr double PosStopF = (2.146E+9f); + constexpr double VelStopF = (16000.0f); + +#pragma pack(1) + + typedef struct + { + float x; + float y; + float z; + } Cartesian; + + typedef struct + { + float quaternion[4]; // quaternion, normalized, (w,x,y,z) + float gyroscope[3]; // angular velocity （unit: rad/s) (raw data) + float accelerometer[3]; // m/(s2) (raw data) + float rpy[3]; // euler angle（unit: rad) + int8_t temperature; + } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift. + + typedef struct + { + uint8_t r; + uint8_t g; + uint8_t b; + } LED; // foot led brightness: 0~255 + + typedef struct + { + uint8_t mode; // motor working mode + float q; // current angle (unit: radian) + float dq; // current velocity (unit: radian/second) + float ddq; // current acc (unit: radian/second*second) + float tauEst; // current estimated output torque (unit: N.m) + float q_raw; // current angle (unit: radian) + float dq_raw; // current velocity (unit: radian/second) + float ddq_raw; + int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) + uint32_t reserve[2]; + } MotorState; // motor feedback + + typedef struct + { + uint8_t mode; // desired working mode + float q; // desired angle (unit: radian) + float dq; // desired velocity (unit: radian/second) + float tau; // desired output torque (unit: N.m) + float Kp; // desired position stiffness (unit: N.m/rad ) + float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) + uint32_t reserve[3]; + } MotorCmd; // motor control + + typedef struct + { + uint8_t levelFlag; // flag to distinguish high level or low level + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + IMU imu; + MotorState motorState[20]; + int16_t footForce[4]; // force sensors + int16_t footForceEst[4]; // force sensors + uint32_t tick; // reference real-time from motion controller (unit: us) + uint8_t wirelessRemote[40]; // wireless commands + uint32_t reserve; + uint32_t crc; + } LowState; // low level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + MotorCmd motorCmd[20]; + LED led[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } LowCmd; // low level control + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; + float progress; + IMU imu; + uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair + float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking + float position[3]; // (unit: m), from own odometry in inertial frame, usually drift + float bodyHeight; // (unit: m, default: 0.28m), + float velocity[3]; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + Cartesian footPosition2Body[4]; // foot position relative to body + Cartesian footSpeed2Body[4]; // foot speed relative to body + int16_t footForce[4]; + int16_t footForceEst[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } HighState; // high level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr) + // 2. target velocity walking (controlled by velocity + yawSpeed) + // 3. target position walking (controlled by position + ypr[0]) + // 4. path mode walking (reserve for future release) + // 5. position stand down. + // 6. position stand up + // 7. damping mode + // 8. recovery stand + // 9. backflip + // 10. jumpYaw + // 11. straightHand + // 12. dance1 + // 13. dance2 + + uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair + uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3 + float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking + float bodyHeight; // (unit: m, default: 0.28m), + float postion[2]; // (unit: m), desired position in inertial frame + float euler[3]; // (unit: rad), roll pitch yaw in stand mode + float velocity[2]; // (unit: m/s), forwardSpeed, sideSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + LED led[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } HighCmd; // high level control + +#pragma pack() + + typedef struct + { + unsigned long long TotalCount; // total loop count + unsigned long long SendCount; // total send count + unsigned long long RecvCount; // total receive count + unsigned long long SendError; // total send error + unsigned long long FlagError; // total flag error + unsigned long long RecvCRCError; // total reveive CRC error + unsigned long long RecvLoseError; // total lose package count + } UDPState; // UDP communication state + + constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd)); + constexpr int HIGH_STATE_LENGTH = (sizeof(HighState)); + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm.h new file mode 100644 index 0000000000000000000000000000000000000000..7f601d5dd554c7eb3c59d297a422fb271dbdf3aa --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm.h @@ -0,0 +1,81 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_LCM_H_ +#define _UNITREE_LEGGED_LCM_H_ + +#include "comm.h" +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr char highCmdChannel[] = "LCM_High_Cmd"; + constexpr char highStateChannel[] = "LCM_High_State"; + constexpr char lowCmdChannel[] = "LCM_Low_Cmd"; + constexpr char lowStateChannel[] = "LCM_Low_State"; + + template + class LCMHandler + { + public: + LCMHandler(){ + pthread_mutex_init(&countMut, NULL); + pthread_mutex_init(&recvMut, NULL); + } + + void onMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel){ + isrunning = true; + + pthread_mutex_lock(&countMut); + counter = 0; + pthread_mutex_unlock(&countMut); + + T *msg = NULL; + msg = (T *)(rbuf->data); + pthread_mutex_lock(&recvMut); + // sourceBuf = *msg; + memcpy(&sourceBuf, msg, sizeof(T)); + pthread_mutex_unlock(&recvMut); + } + + bool isrunning = false; + T sourceBuf = {0}; + pthread_mutex_t countMut; + pthread_mutex_t recvMut; + int counter = 0; + }; + + class LCM { + public: + LCM(uint8_t level); + ~LCM(); + void SubscribeCmd(); + void SubscribeState(); // remember to call this when change control level + int Send(HighCmd&); // lcm send cmd + int Send(LowCmd&); // lcm send cmd + int Send(HighState&); // lcm send state + int Send(LowState&); // lcm send state + int Recv(); // directly save in buffer + void Get(HighCmd&); + void Get(LowCmd&); + void Get(HighState&); + void Get(LowState&); + + LCMHandler highStateLCMHandler; + LCMHandler lowStateLCMHandler; + LCMHandler highCmdLCMHandler; + LCMHandler lowCmdLCMHandler; + private: + uint8_t levelFlag = HIGHLEVEL; // default: high level + lcm::LCM lcm; + lcm::Subscription* subLcm; + int lcmFd; + int LCM_PERIOD = 2000; //default 2ms + }; + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm_server.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm_server.h new file mode 100644 index 0000000000000000000000000000000000000000..be48c34513c4e4979fd2eb0c0fff9d5d71bc50e3 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/lcm_server.h @@ -0,0 +1,107 @@ +/***************************************************************** + Copyright (c) 2021, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_LCM_SERVER_ +#define _UNITREE_LEGGED_LCM_SERVER_ + +#include "comm.h" +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +namespace UNITREE_LEGGED_SDK +{ +// Low command Lcm Server +class Lcm_Server_Low +{ +public: + Lcm_Server_Low() : udp(LOWLEVEL), mylcm(LOWLEVEL){ + udp.InitCmdData(cmd); + } + void UDPRecv(){ + udp.Recv(); + } + void UDPSend(){ + udp.Send(); + } + void LCMRecv(); + void RobotControl(); + + UDP udp; + LCM mylcm; + LowCmd cmd = {0}; + LowState state = {0}; +}; +void Lcm_Server_Low::LCMRecv() +{ + if(mylcm.lowCmdLCMHandler.isrunning){ + pthread_mutex_lock(&mylcm.lowCmdLCMHandler.countMut); + mylcm.lowCmdLCMHandler.counter++; + if(mylcm.lowCmdLCMHandler.counter > 10){ + printf("Error! LCM Time out.\n"); + exit(-1); // can be commented out + } + pthread_mutex_unlock(&mylcm.lowCmdLCMHandler.countMut); + } + mylcm.Recv(); +} +void Lcm_Server_Low::RobotControl() +{ + udp.GetRecv(state); + mylcm.Send(state); + mylcm.Get(cmd); + udp.SetSend(cmd); +} + + + +// High command Lcm Server +class Lcm_Server_High +{ +public: + Lcm_Server_High(): + udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)), + mylcm(HIGHLEVEL){ + udp.InitCmdData(cmd); + } + void UDPRecv(){ + udp.Recv(); + } + void UDPSend(){ + udp.Send(); + } + void LCMRecv(); + void RobotControl(); + + UDP udp; + LCM mylcm; + HighCmd cmd = {0}; + HighState state = {0}; +}; + +void Lcm_Server_High::LCMRecv() +{ + if(mylcm.highCmdLCMHandler.isrunning){ + pthread_mutex_lock(&mylcm.highCmdLCMHandler.countMut); + mylcm.highCmdLCMHandler.counter++; + if(mylcm.highCmdLCMHandler.counter > 10){ + printf("Error! LCM Time out.\n"); + exit(-1); // can be commented out + } + pthread_mutex_unlock(&mylcm.highCmdLCMHandler.countMut); + } + mylcm.Recv(); +} + +void Lcm_Server_High::RobotControl() +{ + udp.GetRecv(state); + mylcm.Send(state); + mylcm.Get(cmd); + udp.SetSend(cmd); +} + + + + +} +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/loop.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/loop.h new file mode 100644 index 0000000000000000000000000000000000000000..e36938c1a2c2588052fbbb66230cd57ab630bf8a --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/loop.h @@ -0,0 +1,57 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_LOOP_H_ +#define _UNITREE_LEGGED_LOOP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + +constexpr int THREAD_PRIORITY = 99; // real-time priority + +typedef boost::function Callback; + +class Loop { +public: + Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} + ~Loop(); + void start(); + void shutdown(); + virtual void functionCB() = 0; + +private: + void entryFunc(); + + std::string _name; + float _period; + int _bindCPU; + bool _bind_cpu_flag = false; + bool _isrunning = false; + std::thread _thread; +}; + +class LoopFunc : public Loop { +public: + LoopFunc(std::string name, float period, const Callback& _cb) + : Loop(name, period), _fp(_cb){} + LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) + : Loop(name, period, bindCPU), _fp(_cb){} + void functionCB() { (_fp)(); } +private: + boost::function _fp; +}; + + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/quadruped.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/quadruped.h new file mode 100644 index 0000000000000000000000000000000000000000..38506962dbf5993a5634b2fbf557795e98736cfe --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/quadruped.h @@ -0,0 +1,53 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_QUADRUPED_H_ +#define _UNITREE_LEGGED_QUADRUPED_H_ + +#include + +using namespace std; + +namespace UNITREE_LEGGED_SDK +{ + +enum class LeggedType { + Aliengo, + A1 +}; + +enum class HighLevelType { + Basic, + Sport +}; + +// string VersionSDK(); +void InitEnvironment(); + + +// definition of each leg and joint +constexpr int FR_ = 0; // leg index +constexpr int FL_ = 1; +constexpr int RR_ = 2; +constexpr int RL_ = 3; + +constexpr int FR_0 = 0; // joint index +constexpr int FR_1 = 1; +constexpr int FR_2 = 2; + +constexpr int FL_0 = 3; +constexpr int FL_1 = 4; +constexpr int FL_2 = 5; + +constexpr int RR_0 = 6; +constexpr int RR_1 = 7; +constexpr int RR_2 = 8; + +constexpr int RL_0 = 9; +constexpr int RL_1 = 10; +constexpr int RL_2 = 11; + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/safety.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/safety.h new file mode 100644 index 0000000000000000000000000000000000000000..a1be0c33502d000c79d77789a1b10253b113abcd --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/safety.h @@ -0,0 +1,31 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_SAFETY_H_ +#define _UNITREE_LEGGED_SAFETY_H_ + +#include "comm.h" +#include "quadruped.h" + +namespace UNITREE_LEGGED_SDK +{ + +class Safety{ +public: + Safety(LeggedType type); + ~Safety(); + void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode + void PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, + means 10%~100% power limit. If you are new, then use 1; if you are familiar, + then can try bigger number or even comment this function. */ + void PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree +private: + int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. + double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; +}; + + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/udp.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/udp.h new file mode 100644 index 0000000000000000000000000000000000000000..f24d73930e62d341826892797298a787905fa894 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/udp.h @@ -0,0 +1,69 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_UDP_H_ +#define _UNITREE_LEGGED_UDP_H_ + +#include "comm.h" +#include "unitree_legged_sdk/quadruped.h" +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr int UDP_CLIENT_PORT = 8080; // local port + constexpr int UDP_SERVER_PORT = 8007; // target port + constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address + constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address + + // Notice: User defined data(like struct) should add crc(4Byte) at the end. + class UDP { + public: + UDP(uint8_t level, HighLevelType highControl = HighLevelType::Basic); // unitree dafault IP and Port + UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, int sendLength, int recvLength, int useTimeOut = -1); + UDP(uint16_t localPort, int sendLength, int recvLength, bool isServer = false); // as server, client IP and port can change + ~UDP(); + void InitCmdData(HighCmd& cmd); + void InitCmdData(LowCmd& cmd); + void SwitchLevel(int level); + void SetDisconnectTime(float callback_dt, float disconnectTime); // disconnect for another IP to connect + void SetAccessibleTime(float callback_dt, float accessibleTime); // if can access data + + int SetSend(HighCmd&); + int SetSend(LowCmd&); + int SetSend(char* cmd); + void GetRecv(HighState&); + void GetRecv(LowState&); + void GetRecv(char*); + int Send(); + int Recv(); // directly save in buffer + + UDPState udpState; + char* targetIP; + uint16_t targetPort; + char* localIP; + uint16_t localPort; + bool accessible = false; // can access or not + int useTimeOut = -1; // use time out method or not, (unit: ms) + bool isServer = false; // server mode with changeable IP/port. SetDisconnectTime() will set this true. + + private: + void init(uint16_t localPort, const char* targetIP, uint16_t targetPort); + + uint8_t levelFlag = HIGHLEVEL; // default: high level + int sockFd; + bool connected; // udp works with connect() function, rather than server mode + int sendLength; + int recvLength; + char* recvBuf; + char* recvSource; + char* sendBuf; + int lose_recv; + pthread_mutex_t sendMut; + pthread_mutex_t recvMut; + }; + +} + +#endif diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_joystick.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_joystick.h new file mode 100644 index 0000000000000000000000000000000000000000..8ea87c04ce321f64db1d68ea2cf77e3898784446 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_joystick.h @@ -0,0 +1,44 @@ +/***************************************************************** +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#ifndef UNITREE_JOYSTICK_H +#define UNITREE_JOYSTICK_H + +#include +// 16b +typedef union { + struct { + uint8_t R1 :1; + uint8_t L1 :1; + uint8_t start :1; + uint8_t select :1; + uint8_t R2 :1; + uint8_t L2 :1; + uint8_t F1 :1; + uint8_t F2 :1; + uint8_t A :1; + uint8_t B :1; + uint8_t X :1; + uint8_t Y :1; + uint8_t up :1; + uint8_t right :1; + uint8_t down :1; + uint8_t left :1; + } components; + uint16_t value; +} xKeySwitchUnion; + +// 40 Byte (now used 24B) +typedef struct { + uint8_t head[2]; + xKeySwitchUnion btn; + float lx; + float rx; + float ry; + float L2; + float ly; + + uint8_t idle[16]; +} xRockerBtnDataStruct; + +#endif // UNITREE_JOYSTICK_H \ No newline at end of file diff --git a/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_legged_sdk.h b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_legged_sdk.h new file mode 100644 index 0000000000000000000000000000000000000000..613b9398bdbce44073da4e3117245052afc3a573 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/include/unitree_legged_sdk/unitree_legged_sdk.h @@ -0,0 +1,18 @@ +/***************************************************************** + Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +******************************************************************/ + +#ifndef _UNITREE_LEGGED_SDK_H_ +#define _UNITREE_LEGGED_SDK_H_ + +#include "comm.h" +#include "safety.h" +#include "udp.h" +#include "loop.h" +#include "lcm.h" +#include "quadruped.h" +#include + +#define UT UNITREE_LEGGED_SDK //short name + +#endif diff --git a/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_amd64.so b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_amd64.so new file mode 100644 index 0000000000000000000000000000000000000000..278aadaaa876bb9fa6395e37602eb7e999915bcf Binary files /dev/null and b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_amd64.so differ diff --git a/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm32.so b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm32.so new file mode 100644 index 0000000000000000000000000000000000000000..f5c180b39c544034c048d7d20ff11137e70294ae Binary files /dev/null and b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm32.so differ diff --git a/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm64.so b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm64.so new file mode 100644 index 0000000000000000000000000000000000000000..3a97245a2d9551f9511205161f0cb82f508f2cce Binary files /dev/null and b/dsoftbus/unitree_softbus_demo/lib/libunitree_legged_sdk_arm64.so differ diff --git a/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_client.cpp b/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_client.cpp new file mode 100644 index 0000000000000000000000000000000000000000..be31521098c36407c3093cd7a82123a35fc2cde9 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_client.cpp @@ -0,0 +1,143 @@ +#include +#include +#include +#include +#include "securec.h" +#include "discovery_service.h" +#include "softbus_bus_center.h" +#include "session.h" +#include "softbus_init.h" +#include "softbus_sdk/robot_walk.h" + +#define PACKAGE_NAME "softbus_sample" +#define LOCAL_SESSION_NAME "Robot_dog" +#define TARGET_SESSION_NAME "hwpad" +#define DEFAULT_CAPABILITY "osdCapability" +#define DEFAULT_SESSION_GROUP "group_test" +#define DEBUG_FLAG_FILE "debug" +#define DEFAULT_PUBLISH_ID 123 + +static SoftClientInitParam g_param = { + .package_name = PACKAGE_NAME, + .local_session_name = LOCAL_SESSION_NAME, + .target_session_name = TARGET_SESSION_NAME, + .default_capability = DEFAULT_CAPABILITY, + .default_session_group = DEFAULT_SESSION_GROUP, + .default_publish_id = DEFAULT_PUBLISH_ID, + .opened_sessionId = -1 +}; + +SoftClientInitParam *param = &g_param; + +static Custom g_custom(HIGHLEVEL); + +int SessionOpened(int sessionId, int result) { + logger("CB: session %d open fail:%d", sessionId, result); + if (result == 0) { + g_param.opened_sessionId = sessionId; + } + return result; +} + +int testDogWithSoftbus() +{ + bool loop = true; + int ret; + LoopFunc loop_control("control_loop", g_custom.dt, boost::bind(&Custom::RobotControl, &g_custom)); + LoopFunc loop_udpSend("udp_send", g_custom.dt, 3, boost::bind(&Custom::UDPSend, &g_custom)); + LoopFunc loop_udpRecv("udp_recv", g_custom.dt, 3, boost::bind(&Custom::UDPRecv, &g_custom)); + + ret = CreateSessionServerInterface(param, SessionOpened); + if (ret) { + logger("CreateSessionServer fail, ret=%d\n", ret); + return ret; + } + + ret = PublishServiceInterface(param); + if (ret) { + logger("PublishService fail, ret=%d\n", ret); + goto err_PublishServiceInterface; + } + + ret = DiscoveryInterface(param); + if (ret) { + logger("DiscoveryInterface fail, ret=%d\n", ret); + goto err_DiscoveryInterface; + } + + loop_udpSend.start(); + loop_udpRecv.start(); + loop_control.start(); + + while (loop) { + logger("\nInput c to commnuication, Input s to stop:"); + char op = getchar(); + switch(op) { + case 'c': + commnunicate(param); + continue; + case 's': + loop = false; + break; + case '\n': + break; + default: + continue; + } + } + + StopDiscoveryInterface(param); +err_DiscoveryInterface: + UnPublishServiceInterface(param); +err_PublishServiceInterface: + RemoveSessionServerInterface(param); + return 0; +} + +int testUnitree() +{ + logger("Communication level is set to HIGH-level.\n"); + logger("WARNING: Make sure the robot is standing on the ground.\n"); + LoopFunc loop_control("control_loop", g_custom.dt, boost::bind(&Custom::RobotControl, &g_custom)); + LoopFunc loop_udpSend("udp_send", g_custom.dt, 3, boost::bind(&Custom::UDPSend, &g_custom)); + LoopFunc loop_udpRecv("udp_recv", g_custom.dt, 3, boost::bind(&Custom::UDPRecv, &g_custom)); + loop_udpSend.start(); + loop_udpRecv.start(); + loop_control.start(); + g_custom.dogStartTest(); + return 0; +} + +void debugMod() { + logger("s to test softbus \nt to test unitree sdk %s"); + char op = getchar(); + switch (op) + { + case 's': + testDogWithSoftbus(); + break; + case 't': + testUnitree(); + break; + case '\n': + break; + default: + break; + } +} + +bool isDebug() { + if (!access(DEBUG_FLAG_FILE, F_OK)) { + return true; + } + return false; +} + +int main(int argc, char **argv) +{ + if (isDebug()) { + debugMod(); + } else { + testDogWithSoftbus(); + } +} diff --git a/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_walk.cpp b/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_walk.cpp new file mode 100644 index 0000000000000000000000000000000000000000..604addaaaa9d79fddd318e77990cf10966ecece2 --- /dev/null +++ b/dsoftbus/unitree_softbus_demo/softbus_demo/client/robot_walk.cpp @@ -0,0 +1,223 @@ +/********************************************************************** + Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. +***********************************************************************/ + +#include +#include +#include +#include "softbus_init.h" +#include "softbus_sdk/robot_walk.h" + +#define DEFAULT_GAIT_TYPE 1 +#define DEFAULT_VELOCITY 0.4f +#define DOING_TIME_SEC 1 + +using namespace UNITREE_LEGGED_SDK; + +void Custom::UDPRecv() +{ + udp.Recv(); +} + +void Custom::UDPSend() +{ + udp.Send(); +} + +void Custom::log(const char *__restrict __format, ...) { + char str[256]; + sprintf(str, __format); + time_t now = time(0); + this->logfile << ctime(&now) << str << "-----" << endl; +} + +void Custom::initStandCmd() { + cmd.mode = 1; // 0:idle, default stand 1:forced stand 2:walk continuously + cmd.gaitType = 0; + cmd.speedLevel = 0; + cmd.footRaiseHeight = 0; + cmd.bodyHeight = 0; + cmd.euler[0] = 0; + cmd.euler[1] = 0; + cmd.euler[2] = 0; + cmd.velocity[0] = 0.0f; + cmd.velocity[1] = 0.0f; + cmd.yawSpeed = 0.0f; + cmd.reserve = 0; +} + +void Custom::walking() { + cmd.mode = 2; // 0:idle, default stand 1:forced stand 2:walk continuously + cmd.gaitType = 0; + cmd.speedLevel = 0; + cmd.footRaiseHeight = 0; + cmd.bodyHeight = 0; + cmd.euler[0] = 0; + cmd.euler[1] = 0; + cmd.euler[2] = 0; + cmd.velocity[0] = 0.0f; + cmd.velocity[1] = 0.0f; + cmd.yawSpeed = 0.0f; + cmd.reserve = 0; + sleep(DOING_TIME_SEC); +} + +void Custom::RobotControl() +{ + motiontime += 2; + udp.GetRecv(state); + + if (motiontime > 0 && motiontime < 500) + { + this->initStandCmd(); + } + + if (motiontime > 1000 && motiontime < 2000) + { + cmd.mode = 6; + } + udp.SetSend(cmd); +} + +void Custom::dogStartTest() +{ + logger("0: STAND_UP\n"); + logger("7: WALK_YAW_LEFT\n"); + logger("9: WALK_YAW_RIGHT\n"); + logger("8: WALK_FORWARD\n"); + logger("2: WALK_BACKWARD\n"); + logger("4: WALK_LEFT\n"); + logger("6: WALK_RIGHT\n"); + ; + int type; + while (1) { + std::cin >> type; + logger( "do dog action : %d\n", type); + this->dogDoAction(type); + logger( "end dog action : %d\n", type); + } +} + +void Custom::RobotYawLeft() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.yawSpeed = -0.8; + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +void Custom::RobotYawRight() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.yawSpeed = 0.8; + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +void Custom::RobotForward() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.velocity[0] = DEFAULT_VELOCITY; // -1 ~ +1 + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +void Custom::RobotLeft() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.velocity[1] = DEFAULT_VELOCITY; + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +// velocity，左正右负 +void Custom::RobotRight() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.velocity[1] = -DEFAULT_VELOCITY; + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +void Custom::RobotBackward() { + cmd.mode = 2; + cmd.gaitType = DEFAULT_GAIT_TYPE; + cmd.footRaiseHeight = 0.05; + cmd.velocity[0] = -DEFAULT_VELOCITY; // -1 ~ +1 + sleep(DOING_TIME_SEC); + this->walking(); + this->initStandCmd(); + return; +} + +void Custom::RobotStandUp() { + cmd.mode = 6; + sleep(DOING_TIME_SEC); + return; +} + +void Custom::RobotSitDown() { + cmd.mode = 5; + sleep(DOING_TIME_SEC); + return; +} + +void Custom::dogDoActionWithSoftbus(string key) +{ + if (this->actionMap.find(key) == actionMap.end()) { + logger("key %s error, not exsit.\n", key); + return; + } + logger("do dog action : %s", key); + this->dogDoAction(this->actionMap[key]); + logger("end : %s", key); +} + +void Custom::dogDoAction(int type) +{ + switch (type) + { + case WALK_YAW_LEFT: + this->RobotYawLeft(); + break; + case WALK_YAW_RIGHT: + this->RobotYawRight(); + break; + case WALK_FORWARD: + this->RobotForward(); + break; + case WALK_BACKWARD: + this->RobotBackward(); + break; + case WALK_LEFT: + this->RobotLeft(); + break; + case WALK_RIGHT: + this->RobotRight(); + break; + case STAND_UP: + this->RobotStandUp(); + break; + case SIT_DOWN: + this->RobotSitDown(); + break; + default: + return; + } +} +