Skip to content

Commit 4f8aca7

Browse files
author
yangning wu
committed
add a2 sport client
1 parent 174541a commit 4f8aca7

File tree

7 files changed

+478
-1
lines changed

7 files changed

+478
-1
lines changed

‎example/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,4 +9,5 @@ add_subdirectory(b2)
99
add_subdirectory(h1)
1010
add_subdirectory(g1)
1111
add_subdirectory(go2w)
12-
add_subdirectory(b2w)
12+
add_subdirectory(b2w)
13+
add_subdirectory(a2)

‎example/a2/CMakeLists.txt‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
add_executable(a2_sport_client a2_sport_client.cpp)
2+
target_link_libraries(a2_sport_client unitree_sdk2)
3+
4+
add_executable(a2_sport_state a2_sport_state.cpp)
5+
target_link_libraries(a2_sport_state unitree_sdk2)

‎example/a2/a2_sport_client.cpp‎

Lines changed: 183 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,183 @@
1+
#include <iostream>
2+
#include <string>
3+
#include <chrono>
4+
#include <mutex>
5+
#include <thread>
6+
#include <pthread.h>
7+
#include <stdexcept>
8+
#include <unitree/robot/a2/sport/sport_client.hpp>
9+
10+
using namespace std;
11+
12+
struct TestOption
13+
{
14+
std::string name;
15+
int id;
16+
};
17+
18+
const vector<TestOption> option_list =
19+
{
20+
{"damp", 0},
21+
{"balance_stand", 1},
22+
{"stop_move", 2},
23+
{"stand_down", 3},
24+
{"recovery_stand", 4},
25+
{"move", 5},
26+
{"switch_gait", 6},
27+
{"speed_level", 7},
28+
{"get_state", 8},
29+
{"recovery_switch", 9},
30+
{"body_height", 10},
31+
{"stand_up", 11},
32+
33+
};
34+
35+
int ConvertToInt(const std::string &str)
36+
{
37+
try
38+
{
39+
std::stoi(str);
40+
return std::stoi(str);
41+
}
42+
catch (const std::invalid_argument &)
43+
{
44+
return -1;
45+
}
46+
catch (const std::out_of_range &)
47+
{
48+
return -1;
49+
}
50+
}
51+
52+
class UserInterface
53+
{
54+
public:
55+
UserInterface() {};
56+
~UserInterface() {};
57+
58+
void terminalHandle()
59+
{
60+
std::string input;
61+
std::getline(std::cin, input);
62+
63+
if (input.compare("list") == 0)
64+
{
65+
for (TestOption option : option_list)
66+
{
67+
std::cout << option.name << ", id: " << option.id << std::endl;
68+
}
69+
}
70+
71+
for (TestOption option : option_list)
72+
{
73+
if (input.compare(option.name) == 0 || ConvertToInt(input) == option.id)
74+
{
75+
test_option_->id = option.id;
76+
test_option_->name = option.name;
77+
std::cout << "Test: " << test_option_->name << ", test_id: " << test_option_->id << std::endl;
78+
}
79+
}
80+
};
81+
82+
TestOption *test_option_;
83+
};
84+
85+
int main(int argc, char **argv)
86+
{
87+
if (argc < 2)
88+
{
89+
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
90+
exit(-1);
91+
}
92+
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
93+
94+
TestOption test_option;
95+
test_option.id = 1;
96+
97+
unitree::robot::a2::SportClient sport_client;
98+
sport_client.SetTimeout(25.0f);
99+
sport_client.Init();
100+
101+
UserInterface user_interface;
102+
user_interface.test_option_ = &test_option;
103+
104+
std::cout << "Input \"list \" to list all test option ..." << std::endl;
105+
long res_count = 0;
106+
while (1)
107+
{
108+
auto time_start_trick = std::chrono::high_resolution_clock::now();
109+
static const constexpr auto dt = std::chrono::microseconds(20000); // 50Hz
110+
111+
user_interface.terminalHandle();
112+
113+
int res = 1;
114+
if (test_option.id == 0)
115+
{
116+
res = sport_client.Damp();
117+
}
118+
else if (test_option.id == 1)
119+
{
120+
res = sport_client.BalanceStand();
121+
}
122+
else if (test_option.id == 2)
123+
{
124+
res = sport_client.StopMove();
125+
}
126+
else if (test_option.id == 3)
127+
{
128+
res = sport_client.StandDown();
129+
}
130+
else if (test_option.id == 4)
131+
{
132+
res = sport_client.RecoveryStand();
133+
}
134+
else if (test_option.id == 5)
135+
{
136+
res = sport_client.Move(0.0, 0.0, 0.5);
137+
}
138+
else if (test_option.id == 6)
139+
{
140+
res = sport_client.SwitchGait(0);
141+
}
142+
else if (test_option.id == 7)
143+
{
144+
res = sport_client.SpeedLevel(1);
145+
}
146+
else if (test_option.id == 8)
147+
{
148+
std::map<std::string, std::string> state_map;
149+
res = sport_client.GetState(state_map);
150+
std::cout << "fsm_id: " << state_map["fsm_id"] << std::endl;
151+
std::cout << "fsm_name: " << state_map["fsm_name"] << std::endl;
152+
std::cout << "speed_level: " << state_map["speed_level"] << std::endl;
153+
std::cout << "auto_recovery_switch: " << state_map["auto_recovery_switch"] << std::endl;
154+
std::cout << "process_state: " << state_map["process_state"] << std::endl;
155+
}
156+
else if (test_option.id == 9)
157+
{
158+
res = sport_client.SetAutoRecovery(0);
159+
}
160+
else if (test_option.id == 10)
161+
{
162+
res = sport_client.BodyHeight(0.3f);
163+
}
164+
else if (test_option.id == 11)
165+
{
166+
res = sport_client.StandUp();
167+
}
168+
169+
170+
if (res < 0)
171+
{
172+
res_count += 1;
173+
std::cout << "Request error for: " << option_list[test_option.id].name << ", code: " << res << ", count: " << res_count << std::endl;
174+
}
175+
else
176+
{
177+
res_count = 0;
178+
std::cout << "Request successed: " << option_list[test_option.id].name << ", code: " << res << std::endl;
179+
}
180+
std::this_thread::sleep_until(time_start_trick + dt);
181+
}
182+
return 0;
183+
}

‎example/a2/a2_sport_state.cpp‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
2+
3+
#include <cmath>
4+
#include <unitree/robot/channel/channel_subscriber.hpp>
5+
#include <unitree/idl/go2/SportModeState_.hpp>
6+
7+
#define TOPIC_HIGHSTATE "rt/sportmodestate"
8+
using namespace unitree::common;
9+
10+
const std::vector<std::string> FSM_STATE_STR = {
11+
"PASSIVE",
12+
"STAND_DOWN",
13+
"STAND_UP",
14+
"DEFAULT_MODE",
15+
"RUNNING_MODE",
16+
"CLIMB_MODE",
17+
"LEFT_SIDE_GAIT",
18+
"RIGHT_SIDE_GAIT",
19+
"HANDSTAND",
20+
"BIPED_STAND",
21+
"FRONT_FLIP",
22+
"BACK_FLIP",
23+
"RECOVERY",
24+
"BASE_HEIGHT_CTRL"};
25+
26+
class Custom
27+
{
28+
public:
29+
Custom()
30+
{
31+
suber.reset(new unitree::robot::ChannelSubscriber<unitree_go::msg::dds_::SportModeState_>(TOPIC_HIGHSTATE));
32+
suber->InitChannel(std::bind(&Custom::HighStateHandler, this, std::placeholders::_1), 1);
33+
};
34+
35+
void HighStateHandler(const void *message)
36+
{
37+
state = *(unitree_go::msg::dds_::SportModeState_ *)message;
38+
39+
std::cout << "Position: " << state.position()[0] << ", " << state.position()[1] << ", " << state.position()[2] << std::endl;
40+
std::cout << "Velocity: " << state.velocity()[0] << ", " << state.velocity()[1] << ", " << state.velocity()[2] << std::endl;
41+
std::cout << "Mode: " << FSM_STATE_STR[int(state.mode())] << std::endl;
42+
std::cout << "Progress: " << state.progress() << std::endl;
43+
44+
// std::cout << "IMU rpy: " << state.imu_state().rpy()[0] << ", " << state.imu_state().rpy()[1] << ", " << state.imu_state().rpy()[2] << std::endl;
45+
};
46+
47+
unitree_go::msg::dds_::SportModeState_ state;
48+
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> suber;
49+
float dt = 0.05; // 控制步长0.001~0.01
50+
};
51+
52+
int main(int argc, char **argv)
53+
{
54+
if (argc < 2)
55+
{
56+
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
57+
exit(-1);
58+
}
59+
60+
unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]);
61+
Custom custom;
62+
63+
while (1)
64+
{
65+
sleep(10);
66+
}
67+
return 0;
68+
}

‎include/unitree/robot/a2/sport/sport_api.hpp‎

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
#ifndef __UT_ROBOT_A2_SPORT_API_HPP__
2+
#define __UT_ROBOT_A2_SPORT_API_HPP__
3+
4+
#include <unitree/common/json/jsonize.hpp>
5+
namespace unitree
6+
{
7+
namespace robot
8+
{
9+
namespace a2
10+
{
11+
/*service name*/
12+
const std::string ROBOT_SPORT_SERVICE_NAME = "sport";
13+
14+
/*api version*/
15+
const std::string ROBOT_SPORT_API_VERSION = "1.0.0.1";
16+
17+
/*api id*/
18+
// Set motion
19+
const int32_t ROBOT_SPORT_API_ID_DAMP = 1001;
20+
const int32_t ROBOT_SPORT_API_ID_BALANCESTAND = 1002;
21+
const int32_t ROBOT_SPORT_API_ID_STOPMOVE = 1003;
22+
const int32_t ROBOT_SPORT_API_ID_STANDUP = 1004;
23+
const int32_t ROBOT_SPORT_API_ID_STANDDOWN = 1005;
24+
const int32_t ROBOT_SPORT_API_ID_RECOVERYSTAND = 1006;
25+
const int32_t ROBOT_SPORT_API_ID_EULER = 1007;
26+
const int32_t ROBOT_SPORT_API_ID_MOVE = 1008;
27+
const int32_t ROBOT_SPORT_API_ID_SWITCHGAIT = 1011;
28+
const int32_t ROBOT_SPORT_API_ID_BODYHEIGHT = 1013;
29+
const int32_t ROBOT_SPORT_API_ID_SPEEDLEVEL = 1015;
30+
const int32_t ROBOT_SPORT_API_ID_SETAUTORECOVERY = 1040;
31+
32+
// Get state
33+
const int32_t ROBOT_SPORT_API_ID_GETSTATE = 1034;
34+
35+
}
36+
37+
}
38+
}
39+
40+
#endif //__UT_ROBOT_A2_SPORT_API_HPP__

0 commit comments

Comments
 (0)