chengming
/
unitree_ros_to_real
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix ROS<>LCM message converter for Unitree SDK v3.5.0

This commit is contained in:
Kei Okada 2022-03-29 17:19:56 +09:00
parent 3c0fe3c97d
commit 49acae319f
1 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
unitree_legged_real/include/convert.h
View File

@ -105,9 +105,9 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
{ {
unitree_legged_msgs::LowState ros; unitree_legged_msgs::LowState ros;
ros.levelFlag = lcm.levelFlag; ros.levelFlag = lcm.levelFlag;
ros.commVersion = lcm.commVersion; ros.commVersion = lcm.version[0];
ros.robotID = lcm.robotID; ros.robotID = lcm.version[1];
ros.SN = lcm.SN; ros.SN = lcm.SN[0];
ros.bandWidth = lcm.bandWidth; ros.bandWidth = lcm.bandWidth;
ros.imu = ToRos(lcm.imu); ros.imu = ToRos(lcm.imu);
for(int i = 0; i<20; i++){ for(int i = 0; i<20; i++){
@ -131,9 +131,9 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGE
{ {
UNITREE_LEGGED_SDK::LowCmd lcm; UNITREE_LEGGED_SDK::LowCmd lcm;
lcm.levelFlag = ros.levelFlag; lcm.levelFlag = ros.levelFlag;
lcm.commVersion = ros.commVersion; lcm.version[0] = ros.commVersion;
lcm.robotID = ros.robotID; lcm.version[1] = ros.robotID;
lcm.SN = ros.SN; lcm.SN[0] = ros.SN;
lcm.bandWidth = ros.bandWidth; lcm.bandWidth = ros.bandWidth;
for(int i = 0; i<20; i++){ for(int i = 0; i<20; i++){
lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
@ -151,9 +151,9 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
{ {
unitree_legged_msgs::HighState ros; unitree_legged_msgs::HighState ros;
ros.levelFlag = lcm.levelFlag; ros.levelFlag = lcm.levelFlag;
ros.commVersion = lcm.commVersion; ros.commVersion = lcm.version[0];
ros.robotID = lcm.robotID; ros.robotID = lcm.version[1];
ros.SN = lcm.SN; ros.SN = lcm.SN[0];
ros.bandWidth = lcm.bandWidth; ros.bandWidth = lcm.bandWidth;
ros.mode = lcm.mode; ros.mode = lcm.mode;
ros.progress = lcm.progress; ros.progress = lcm.progress;
@ -193,9 +193,9 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG
{ {
UNITREE_LEGGED_SDK::HighCmd lcm; UNITREE_LEGGED_SDK::HighCmd lcm;
lcm.levelFlag = ros.levelFlag; lcm.levelFlag = ros.levelFlag;
lcm.commVersion = ros.commVersion; lcm.version[0] = ros.commVersion;
lcm.robotID = ros.robotID; lcm.version[1] = ros.robotID;
lcm.SN = ros.SN; lcm.SN[0] = ros.SN;
lcm.bandWidth = ros.bandWidth; lcm.bandWidth = ros.bandWidth;
lcm.mode = ros.mode; lcm.mode = ros.mode;
lcm.gaitType = ros.gaitType; lcm.gaitType = ros.gaitType;
@ -229,4 +229,4 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG
return lcm; return lcm;
} }
#endif // _CONVERT_H_ #endif // _CONVERT_H_