make custom class and callback function lighter
This commit is contained in:
parent
1ad059e38d
commit
200adc6e3b
|
@ -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_
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
@ -132,6 +132,7 @@ int main(int argc, char **argv)
|
|||
|
||||
pub.publish(high_cmd_ros);
|
||||
|
||||
ros::spinOnce();
|
||||
loop_rate.sleep();
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
Loading…
Reference in New Issue