83 lines
2.4 KiB
C++
83 lines
2.4 KiB
C++
|
#include <unitree/robot/go2/robot_state/robot_state_client.hpp>
|
||
|
|
#include <unitree/common/time/time_tool.hpp>
|
||
|
|
|
||
|
|
using namespace unitree::common;
|
||
|
|
using namespace unitree::robot;
|
||
|
|
using namespace unitree::robot::go2;
|
||
|
|
|
||
|
|
int main(int32_t argc, const char** argv)
|
||
|
|
{
|
||
|
|
if (argc < 2)
|
||
|
|
{
|
||
|
|
std::cout << "Usage: robot_state_client_example [NetWorkInterface(eth0)] [ServiceName(sport_mode)]" << std::endl;
|
||
|
|
exit(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string networkInterface = "eth0", serviceName = "sport_mode";
|
||
|
|
|
||
|
|
if (argc > 1)
|
||
|
|
{
|
||
|
|
networkInterface = argv[1];
|
||
|
|
}
|
||
|
|
|
||
|
|
if (argc > 2)
|
||
|
|
{
|
||
|
|
serviceName = argv[2];
|
||
|
|
}
|
||
|
|
|
||
|
|
std::cout << "NetWorkInterface:" << networkInterface << std::endl;
|
||
|
|
std::cout << "Switch ServiceName:" << serviceName << std::endl;
|
||
|
|
|
||
|
|
ChannelFactory::Instance()->Init(0, networkInterface);
|
||
|
|
|
||
|
|
RobotStateClient rsc;
|
||
|
|
rsc.SetTimeout(10.0f);
|
||
|
|
rsc.Init();
|
||
|
|
|
||
|
|
std::string clientApiVersion = rsc.GetApiVersion();
|
||
|
|
std::string serverApiVersion = rsc.GetServerApiVersion();
|
||
|
|
|
||
|
|
if (clientApiVersion != serverApiVersion)
|
||
|
|
{
|
||
|
|
std::cout << "client and server api versions are not equal." << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
Timer timer;
|
||
|
|
|
||
|
|
int32_t status;
|
||
|
|
int32_t ret = rsc.SetReportFreq(3, 30);
|
||
|
|
std::cout << "Call SetReportFreq[3,30] ret:" << ret << ", cost:" << timer.Stop() << " (us)" << std::endl;
|
||
|
|
|
||
|
|
sleep(5);
|
||
|
|
timer.Restart();
|
||
|
|
|
||
|
|
ret = rsc.ServiceSwitch(serviceName, 0, status);
|
||
|
|
std::cout << "Call ServiceSwitch[" << serviceName << ",0] ret:" << ret << ", cost:" << timer.Stop() << " (us)" << std::endl;
|
||
|
|
|
||
|
|
sleep(5);
|
||
|
|
timer.Restart();
|
||
|
|
|
||
|
|
ret = rsc.ServiceSwitch(serviceName, 1, status);
|
||
|
|
std::cout << "Call ServiceSwitch[" << serviceName << ",1] ret:" << ret << ", cost:" << timer.Stop() << " (us)" << std::endl;
|
||
|
|
|
||
|
|
sleep(5);
|
||
|
|
timer.Restart();
|
||
|
|
|
||
|
|
std::vector<ServiceState> serviceStateList;
|
||
|
|
ret = rsc.ServiceList(serviceStateList);
|
||
|
|
std::cout << "Call ServiceList ret:" << ret << ", cost:" << timer.Stop() << " (us)" << std::endl;
|
||
|
|
|
||
|
|
size_t i, count=serviceStateList.size();
|
||
|
|
std::cout << "serviceStateList size:" << count << std::endl;
|
||
|
|
|
||
|
|
for (i=0; i<count; i++)
|
||
|
|
{
|
||
|
|
const ServiceState& serviceState = serviceStateList[i];
|
||
|
|
std::cout << "name:" << serviceState.name << ", status:" << serviceState.status << ", protect:" << serviceState.protect << std::endl;
|
||
|
|
}
|
||
|
|
|
||
|
|
ChannelFactory::Instance()->Release();
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|