forked from unitreerobotics/unitree_ros_to_real
-
Notifications
You must be signed in to change notification settings - Fork 0
/
ros_udp.cpp
164 lines (117 loc) · 3.81 KB
/
ros_udp.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
#include <ros/ros.h>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "convert.h"
#include <chrono>
#include <pthread.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace UNITREE_LEGGED_SDK;
char udp_ip[16];
class Custom
{
public:
UDP low_udp;
UDP high_udp;
HighCmd high_cmd = {0};
HighState high_state = {0};
LowCmd low_cmd = {0};
LowState low_state = {0};
public:
Custom()
: low_udp(LOWLEVEL),
high_udp(8090, const_cast<const char*>(udp_ip), 8082, sizeof(HighCmd), sizeof(HighState))
{
high_udp.InitCmdData(high_cmd);
low_udp.InitCmdData(low_cmd);
}
void highUdpSend()
{
// printf("high udp send is running\n");
high_udp.SetSend(high_cmd);
high_udp.Send();
}
void lowUdpSend()
{
low_udp.SetSend(low_cmd);
low_udp.Send();
}
void lowUdpRecv()
{
low_udp.Recv();
low_udp.GetRecv(low_state);
}
void highUdpRecv()
{
// printf("high udp recv is running\n");
high_udp.Recv();
high_udp.GetRecv(high_state);
}
};
Custom* custom;
ros::Subscriber sub_high;
ros::Subscriber sub_low;
ros::Publisher pub_high;
ros::Publisher pub_low;
long high_count = 0;
long low_count = 0;
void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
{
printf("highCmdCallback is running !\t%ld\n", ::high_count);
custom->high_cmd = rosMsg2Cmd(msg);
unitree_legged_msgs::HighState high_state_ros;
high_state_ros = state2rosMsg(custom->high_state);
pub_high.publish(high_state_ros);
printf("highCmdCallback ending !\t%ld\n\n", ::high_count++);
}
void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
{
printf("lowCmdCallback is running !\t%ld\n", low_count);
custom->low_cmd = rosMsg2Cmd(msg);
unitree_legged_msgs::LowState low_state_ros;
low_state_ros = state2rosMsg(custom->low_state);
pub_low.publish(low_state_ros);
printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "ros_udp");
ros::NodeHandle nh;
std::string ip_string;
nh.param<std::string>("/UDP_IP", ip_string, "192.168.123.161");
strcpy(udp_ip, ip_string.c_str());
printf("Variable udp_ip: %s\n", udp_ip);
custom = new Custom();
// printf("Parameter /UDP_IP: %s%s\n", udp_ip, nh.hasParam("/UDP_IP") ? "" : " (as default value)");
if (strcasecmp(argv[1], "LOWLEVEL") == 0)
{
sub_low = nh.subscribe("low_cmd", 1, lowCmdCallback);
pub_low = nh.advertise<unitree_legged_msgs::LowState>("low_state", 1);
LoopFunc loop_udpSend("low_udp_send", 0.002, 3, boost::bind(&Custom::lowUdpSend, custom));
LoopFunc loop_udpRecv("low_udp_recv", 0.002, 3, boost::bind(&Custom::lowUdpRecv, custom));
loop_udpSend.start();
loop_udpRecv.start();
ros::spin();
// printf("low level runing!\n");
}
else if (strcasecmp(argv[1], "HIGHLEVEL") == 0)
{
sub_high = nh.subscribe("high_cmd", 1, highCmdCallback);
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, custom));
LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, custom));
loop_udpSend.start();
loop_udpRecv.start();
ros::spin();
// printf("high level runing!\n");
}
else
{
std::cout << "Control level name error! Can only be highlevel or lowlevel(not case sensitive)" << std::endl;
exit(-1);
}
return 0;
}