From 200adc6e3b1458b3d6c95f27ea4cee237d5e63cc Mon Sep 17 00:00:00 2001 From: Your Name Date: Wed, 25 May 2022 11:31:06 +0800 Subject: [PATCH] make custom class and callback function lighter --- unitree_legged_real/include/convert.h | 77 ------------------- .../src/exe/example_position.cpp | 3 +- unitree_legged_real/src/exe/example_walk.cpp | 1 + unitree_legged_real/src/exe/ros_udp.cpp | 61 ++++++++++++++- 4 files changed, 59 insertions(+), 83 deletions(-) diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h index 4bd6f7d..9712648 100755 --- a/unitree_legged_real/include/convert.h +++ b/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_ \ No newline at end of file diff --git a/unitree_legged_real/src/exe/example_position.cpp b/unitree_legged_real/src/exe/example_position.cpp index 2c2a6cf..7926577 100644 --- a/unitree_legged_real/src/exe/example_position.cpp +++ b/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(); } diff --git a/unitree_legged_real/src/exe/example_walk.cpp b/unitree_legged_real/src/exe/example_walk.cpp index c56f158..ce46ddb 100644 --- a/unitree_legged_real/src/exe/example_walk.cpp +++ b/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(); } diff --git a/unitree_legged_real/src/exe/ros_udp.cpp b/unitree_legged_real/src/exe/ros_udp.cpp index 3aeb5ac..c16961b 100644 --- a/unitree_legged_real/src/exe/ros_udp.cpp +++ b/unitree_legged_real/src/exe/ros_udp.cpp @@ -8,6 +8,59 @@ #include #include +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);