make custom class and callback function lighter

This commit is contained in:
Your Name 2022-05-25 11:31:06 +08:00
parent 1ad059e38d
commit 200adc6e3b
4 changed files with 59 additions and 83 deletions

View File

@ -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_

View File

@ -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();
}

View File

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

View File

@ -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);