make custom class and callback function lighter

unitree_legged_real/include/convert.h

@@ -300,81 +300,4 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state
     return ros_msg;
 }
 
-using namespace UNITREE_LEGGED_SDK;
-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, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
-    {
-        high_udp.InitCmdData(high_cmd);
-        low_udp.InitCmdData(low_cmd);
-    }
-
-    void cmdAssignment(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
-    {
-
-        high_cmd = rosMsg2Cmd(msg);
-    }
-
-    void cmdAssignment(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
-    {
-
-        low_cmd = rosMsg2Cmd(msg);
-    }
-
-    void stateAssignment(unitree_legged_msgs::LowState &low_state_ros)
-    {
-
-        low_state_ros = state2rosMsg(low_state);
-    }
-
-    void stateAssignment(unitree_legged_msgs::HighState &high_state_ros)
-    {
-
-        high_state_ros = state2rosMsg(high_state);
-    }
-
-    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);
-    }
-};
-
 #endif // _CONVERT_H_

unitree_legged_real/src/exe/example_position.cpp

@@ -76,8 +76,6 @@ int main(int argc, char **argv)
             low_cmd_ros.motorCmd[FR_1].Kd = 1.0;
         }
 
-        ros::spinOnce();
-
         count++;
         if (count > 10)
         {
@@ -87,6 +85,7 @@ int main(int argc, char **argv)
 
         pub.publish(low_cmd_ros);
 
+        ros::spinOnce();
         loop_rate.sleep();
     }
 

unitree_legged_real/src/exe/example_walk.cpp

@@ -132,6 +132,7 @@ int main(int argc, char **argv)
 
         pub.publish(high_cmd_ros);
 
+        ros::spinOnce();
         loop_rate.sleep();
     }
 

unitree_legged_real/src/exe/ros_udp.cpp

@@ -8,6 +8,59 @@
 #include <chrono>
 #include <pthread.h>
 
+using namespace UNITREE_LEGGED_SDK;
+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, "192.168.123.161", 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;
@@ -23,11 +76,11 @@ void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
 {
     printf("highCmdCallback is running !\t%ld\n", ::high_count);
 
-    custom.cmdAssignment(msg);
+    custom.high_cmd = rosMsg2Cmd(msg);
 
     unitree_legged_msgs::HighState high_state_ros;
 
-    custom.stateAssignment(high_state_ros);
+    high_state_ros = state2rosMsg(custom.high_state);
 
     pub_high.publish(high_state_ros);
 
@@ -39,11 +92,11 @@ void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
 
     printf("lowCmdCallback is running !\t%ld\n", low_count);
 
-    custom.cmdAssignment(msg);
+    custom.low_cmd = rosMsg2Cmd(msg);
 
     unitree_legged_msgs::LowState low_state_ros;
 
-    custom.stateAssignment(low_state_ros);
+    low_state_ros = state2rosMsg(custom.low_state);
 
     pub_low.publish(low_state_ros);