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

v3.8.3 support B1

This commit is contained in:
LuoBin 2023-01-17 02:35:00 -05:00
parent c54489e721
commit e3e96db914
13 changed files with 317 additions and 614 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
README.md
View File

@ -1,9 +1,9 @@
Packages Version: v3.8.0 Packages Version: v3.8.3
# Introduction # Introduction
This package can send control command to real robot from ROS. You can do low-level control(namely control all joints on robot) and high-level control(namely control the walking direction and speed of robot). This package can send control command to real robot from ROS. You can do low-level control(namely control all joints on robot) and high-level control(namely control the walking direction and speed of robot).
This version is suitable for unitree_legged_sdk v3.5.1, namely Go1 robot. This version is suitable for unitree_legged_sdk v3.8.3, namely B1 robot.
## Packages: ## Packages:
@ -12,51 +12,24 @@ Basic message function: `unitree_legged_msgs`
The interface between ROS and real robot: `unitree_legged_real` The interface between ROS and real robot: `unitree_legged_real`
## Environment ## Environment
We recommand users to run this package in Ubuntu 18.04 and ROS melodic environment We recommand users to run this package in Ubuntu 20.04 and ROS neotic environment
## Dependencies ## s
* [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk/releases) * [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk/releases)
### Notice ### Notice
The newest release [v3.8.0](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.8.0) only supports for robot: Go1. The release [v3.8.3](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.8.3) only supports for robot: B1.
Check release [v3.3.4](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.3.4) for A1 support.
# Configuration
Before compiling this package, please download the corresponding unitree_legged_sdk as noted above, and put it to your own workspace's source folder(e.g. `~/catkin_ws/src`). Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag.
# Build # Build
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then: You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then download the corresponding unitree_legged_sdk into `~/catkin_ws/src/unitree_ros_to_real`.Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag:
``` ```
cd ~/catkin_ws cd ~/catkin_ws
catkin_make catkin_make
``` ```
# Setup the net connection
First, please connect the network cable between your PC and robot. Then run `ifconfig` in a terminal, you will find your port name. For example, `enx000ec6612921`.
Then, open the `ipconfig.sh` file under the folder `unitree_legged_real`, modify the port name to your own. And run the following commands:
```
sudo chmod +x ipconfig.sh
sudo ./ipconfig.sh
```
If you run the `ifconfig` again, you will find that port has `inet` and `netmask` now.
In order to set your port automatically, you can modify `interfaces`:
```
sudo gedit /etc/network/interfaces
```
And add the following 4 lines at the end:
```
auto enx000ec6612921
iface enx000ec6612921 inet static
address 192.168.123.162
netmask 255.255.255.0
```
Where the port name have to be changed to your own.
# Run the package # Run the package
You can control your real Go1 robot from ROS by this package. You can control your real B1 robot from ROS by this package.
Before you run expamle program, please run command Before you run expamle program, please run
``` ```
roslaunch unitree_legged_real real.launch ctrl_level:=highlevel roslaunch unitree_legged_real real.launch ctrl_level:=highlevel
@ -68,25 +41,15 @@ roslaunch unitree_legged_real real.launch ctrl_level:=lowlevel
It depends which control mode you want to use. It depends which control mode you want to use.
Then, if you want to run high-level control mode, you can run example_walk node like this Then, if you want to run high-level control mode, you can run example_walk node
``` ```
rosrun unitree_legged_real example_walk rosrun unitree_legged_real example_walk
``` ```
If you want to run low-level control mode, you can run example_position program node like this If you want to run low-level control mode, you can run example_position program node
``` ```
rosrun unitree_legged_real example_postion rosrun unitree_legged_real example_postion
``` ```
You can also run the node state_sub to subscribe the feedback information from Go1 robot
```
rosrun unitree_legged_real state_sub
```
You can also run the launch file that enables you control robot via keyboard like you can do in turtlesim package
```
roslaunch unitree_legged_real keyboard_control.launch
```
And before you do the low-level control, please press L2+A to sit the robot down and then press L1+L2+start to make the robot into And before you do the low-level control, please press L2+A to sit the robot down and then press L1+L2+start to make the robot into
mode in which you can do joint-level control, finally make sure you hang the robot up before you run low-level control. mode in which you can do low-level control, finally make sure you hang the robot up before you run low-level control.

6
unitree_legged_msgs/msg/BmsState.msg
View File

@ -4,6 +4,6 @@ uint8 bms_status
uint8 SOC # SOC 0-100% uint8 SOC # SOC 0-100%
int32 current # mA int32 current # mA
uint16 cycle uint16 cycle
int8[2] BQ_NTC # x1 degrees centigrade int8[8] BQ_NTC # x1 degrees centigrade
int8[2] MCU_NTC # x1 degrees centigrade int8[8] MCU_NTC # x1 degrees centigrade
uint16[10] cell_vol # cell voltage mV uint16[30] cell_vol # cell voltage mV

4
unitree_legged_msgs/msg/HighCmd.msg
View File

@ -14,7 +14,9 @@ float32 bodyHeight
float32[2] position float32[2] position
float32[3] euler float32[3] euler
float32[2] velocity float32[2] velocity
float32 yawSpeed float32 yawSpeed
float32[2] dComXy
float32[2] dstandFootXy
BmsCmd bms BmsCmd bms
LED[4] led LED[4] led
uint8[40] wirelessRemote uint8[40] wirelessRemote

7
unitree_legged_msgs/msg/LowState.msg
View File

@ -16,10 +16,5 @@ uint8[40] wirelessRemote
uint32 reserve uint32 reserve
uint32 crc uint32 crc
# Old version Aliengo does not have:
Cartesian[4] eeForceRaw
Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization
Cartesian position # will delete
Cartesian velocity # will delete
Cartesian velocity_w # will delete

19
unitree_legged_real/CMakeLists.txt
View File

@ -18,7 +18,7 @@ else()
set(ARCH arm64) set(ARCH arm64)
endif() endif()
link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH}) link_directories(${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/lib/cpp/${ARCH})
set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) set(EXTRA_LIBS -pthread libunitree_legged_sdk.so)
@ -27,16 +27,11 @@ set(CMAKE_CXX_FLAGS "-O3 -fPIC")
include_directories( include_directories(
include include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
${CMAKE_SOURCE_DIR}/unitree_legged_sdk/include ${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/include
) )
add_executable(example_walk src/exe/example_walk.cpp) add_executable(example_walk src/exe/example_walk.cpp)
target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES}) target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@ -45,19 +40,9 @@ add_executable(example_position src/exe/example_position.cpp)
target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES}) target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(state_sub src/exe/state_sub.cpp)
target_link_libraries(state_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(state_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(ros_udp src/exe/ros_udp.cpp) add_executable(ros_udp src/exe/ros_udp.cpp)
target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES}) target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(ros_udp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(ros_udp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(control_via_keyboard src/exe/control_via_keyboard.cpp)
target_link_libraries(control_via_keyboard ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(control_via_keyboard ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(twist_sub src/exe/twist_sub.cpp)
target_link_libraries(twist_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(twist_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

361
unitree_legged_real/include/convert.h
View File

@ -6,18 +6,20 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#ifndef _CONVERT_H_ #ifndef _CONVERT_H_
#define _CONVERT_H_ #define _CONVERT_H_
#include <unitree_legged_msgs/LowCmd.h> #include "unitree_legged_msgs/BmsCmd.h"
#include <unitree_legged_msgs/LowState.h> #include "unitree_legged_msgs/BmsState.h"
#include <unitree_legged_msgs/HighCmd.h> #include "unitree_legged_msgs/Cartesian.h"
#include <unitree_legged_msgs/HighState.h> #include "unitree_legged_msgs/HighCmd.h"
#include <unitree_legged_msgs/MotorCmd.h> #include "unitree_legged_msgs/HighState.h"
#include <unitree_legged_msgs/MotorState.h> #include "unitree_legged_msgs/IMU.h"
#include <unitree_legged_msgs/BmsCmd.h> #include "unitree_legged_msgs/LED.h"
#include <unitree_legged_msgs/BmsState.h> #include "unitree_legged_msgs/LowCmd.h"
#include <unitree_legged_msgs/IMU.h> #include "unitree_legged_msgs/LowState.h"
#include "unitree_legged_msgs/MotorCmd.h"
#include "unitree_legged_msgs/MotorState.h"
#include "unitree_legged_sdk/unitree_legged_sdk.h" #include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <ros/ros.h> #include "ros/ros.h"
#include <geometry_msgs/Twist.h>
UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
{ {
@ -25,57 +27,75 @@ UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
cmd.off = msg.off; cmd.off = msg.off;
for (int i(0); i < 3; i++) for (std::size_t i(0); i < 3; i++)
{
cmd.reserve[i] = msg.reserve[i]; cmd.reserve[i] = msg.reserve[i];
}
return cmd; return cmd;
} }
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg) unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
{ {
UNITREE_LEGGED_SDK::HighCmd cmd; unitree_legged_msgs::BmsState ros_msg;
for (int i(0); i < 2; i++) for (std::size_t i(0); i < 8; i++)
{ {
cmd.head[i] = msg->head[i]; ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
cmd.SN[i] = msg->SN[i]; ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
cmd.version[i] = msg->version[i];
cmd.position[i] = msg->position[i];
cmd.velocity[i] = msg->velocity[i];
} }
for (int i(0); i < 3; i++) for (std::size_t i(0); i < 30; i++)
ros_msg.cell_vol[i] = state.cell_vol[i];
ros_msg.version_h = state.version_h;
ros_msg.version_l = state.version_l;
ros_msg.bms_status = state.bms_status;
ros_msg.SOC = state.SOC;
ros_msg.current = state.current;
ros_msg.cycle = state.cycle;
return ros_msg;
}
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
{
unitree_legged_msgs::Cartesian ros_msg;
ros_msg.x = state.x;
ros_msg.y = state.y;
ros_msg.z = state.z;
return ros_msg;
}
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
{
unitree_legged_msgs::IMU ros_msg;
for (std::size_t i(0); i < 4; i++)
{ {
cmd.euler[i] = msg->euler[i]; ros_msg.quaternion[i] = state.quaternion[i];
} }
for (int i(0); i < 4; i++) for (std::size_t i(0); i < 3; i++)
{ {
cmd.led[i].r = msg->led[i].r; ros_msg.gyroscope[i] = state.gyroscope[i];
cmd.led[i].g = msg->led[i].g; ros_msg.accelerometer[i] = state.accelerometer[i];
cmd.led[i].b = msg->led[i].b; ros_msg.rpy[i] = state.rpy[i];
} }
for (int i(0); i < 40; i++) ros_msg.temperature = state.temperature;
{
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
}
cmd.levelFlag = msg->levelFlag; return ros_msg;
cmd.frameReserve = msg->frameReserve; }
cmd.bandWidth = msg->bandWidth;
cmd.mode = msg->mode;
cmd.gaitType = msg->gaitType;
cmd.speedLevel = msg->speedLevel;
cmd.footRaiseHeight = msg->footRaiseHeight;
cmd.bodyHeight = msg->bodyHeight;
cmd.yawSpeed = msg->yawSpeed;
cmd.reserve = msg->reserve;
cmd.crc = msg->crc;
cmd.bms = rosMsg2Cmd(msg->bms); UNITREE_LEGGED_SDK::LED rosMsg2Cmd(const unitree_legged_msgs::LED &msg)
{
UNITREE_LEGGED_SDK::LED cmd;
cmd.r = msg.r;
cmd.g = msg.g;
cmd.b = msg.b;
return cmd; return cmd;
} }
@ -91,7 +111,7 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
cmd.Kp = msg.Kp; cmd.Kp = msg.Kp;
cmd.Kd = msg.Kd; cmd.Kd = msg.Kd;
for (int i(0); i < 3; i++) for (std::size_t i(0); i < 3; i++)
{ {
cmd.reserve[i] = msg.reserve[i]; cmd.reserve[i] = msg.reserve[i];
} }
@ -99,37 +119,6 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
return cmd; return cmd;
} }
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
{
UNITREE_LEGGED_SDK::LowCmd cmd;
for (int i(0); i < 2; i++)
{
cmd.head[i] = msg->head[i];
cmd.SN[i] = msg->SN[i];
cmd.version[i] = msg->version[i];
}
for (int i(0); i < 40; i++)
{
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
}
for (int i(0); i < 20; i++)
{
cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]);
}
cmd.bms = rosMsg2Cmd(msg->bms);
cmd.levelFlag = msg->levelFlag;
cmd.frameReserve = msg->frameReserve;
cmd.bandWidth = msg->bandWidth;
cmd.reserve = msg->reserve;
cmd.crc = msg->crc;
return cmd;
}
unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state) unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state)
{ {
@ -151,113 +140,62 @@ unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &sta
return ros_msg; return ros_msg;
} }
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state) UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd &msg)
{ {
unitree_legged_msgs::IMU ros_msg; UNITREE_LEGGED_SDK::HighCmd cmd;
for (int i(0); i < 4; i++) for (std::size_t i(0); i < 2; i++)
{ {
ros_msg.quaternion[i] = state.quaternion[i]; cmd.head[i] = msg.head[i];
cmd.SN[i] = msg.SN[i];
cmd.version[i] = msg.version[i];
cmd.position[i] = msg.position[i];
cmd.velocity[i] = msg.velocity[i];
cmd.dComXy[i] = msg.dComXy[i];
cmd.dstandFootXy[i] = msg.dstandFootXy[i];
} }
for (int i(0); i < 3; i++) for (std::size_t i(0); i < 3; i++)
{ cmd.euler[i] = msg.euler[i];
ros_msg.gyroscope[i] = state.gyroscope[i];
ros_msg.accelerometer[i] = state.accelerometer[i];
ros_msg.rpy[i] = state.rpy[i];
}
ros_msg.temperature = state.temperature; for (std::size_t i(0); i < 4; i++)
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
return ros_msg; for (std::size_t i(0); i < 40; i++)
} cmd.wirelessRemote[i] = msg.wirelessRemote[i];
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state) cmd.levelFlag = msg.levelFlag;
{ cmd.frameReserve = msg.frameReserve;
unitree_legged_msgs::BmsState ros_msg; cmd.bandWidth = msg.bandWidth;
cmd.mode = msg.mode;
cmd.gaitType = msg.gaitType;
cmd.speedLevel = msg.speedLevel;
cmd.footRaiseHeight = msg.footRaiseHeight;
cmd.bodyHeight = msg.bodyHeight;
cmd.yawSpeed = msg.yawSpeed;
cmd.reserve = msg.reserve;
cmd.crc = msg.crc;
for (int i(0); i < 2; i++) cmd.bms = rosMsg2Cmd(msg.bms);
{
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
}
for (int i(0); i < 10; i++) return cmd;
{
ros_msg.cell_vol[i] = state.cell_vol[i];
}
ros_msg.version_h = state.version_h;
ros_msg.version_l = state.version_l;
ros_msg.bms_status = state.bms_status;
ros_msg.SOC = state.SOC;
ros_msg.current = state.current;
ros_msg.cycle = state.cycle;
return ros_msg;
}
unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state)
{
unitree_legged_msgs::LowState ros_msg;
for (int i(0); i < 2; i++)
{
ros_msg.head[i] = state.head[i];
ros_msg.SN[i] = state.SN[i];
ros_msg.version[i] = state.version[i];
}
for (int i(0); i < 4; i++)
{
ros_msg.footForce[i] = state.footForce[i];
ros_msg.footForceEst[i] = state.footForceEst[i];
}
for (int i(0); i < 40; i++)
{
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
}
for (int i(0); i < 20; i++)
{
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
}
ros_msg.imu = state2rosMsg(state.imu);
ros_msg.bms = state2rosMsg(state.bms);
ros_msg.tick = state.tick;
ros_msg.reserve = state.reserve;
ros_msg.crc = state.crc;
return ros_msg;
}
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
{
unitree_legged_msgs::Cartesian ros_msg;
ros_msg.x = state.x;
ros_msg.y = state.y;
ros_msg.z = state.z;
return ros_msg;
} }
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state) unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
{ {
unitree_legged_msgs::HighState ros_msg; unitree_legged_msgs::HighState ros_msg;
for (int i(0); i < 2; i++) for (std::size_t i(0); i < 2; i++)
{ {
ros_msg.head[i] = state.head[i]; ros_msg.head[i] = state.head[i];
ros_msg.SN[i] = state.SN[i]; ros_msg.SN[i] = state.SN[i];
ros_msg.version[i] = state.version[i]; ros_msg.version[i] = state.version[i];
} }
for (int i(0); i < 4; i++) for (std::size_t i(0); i < 4; i++)
{ {
ros_msg.footForce[i] = state.footForce[i]; ros_msg.footForce[i] = state.footForce[i];
ros_msg.footForceEst[i] = state.footForceEst[i]; ros_msg.footForceEst[i] = state.footForceEst[i];
@ -266,18 +204,18 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]); ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
} }
for (int i(0); i < 3; i++) for (std::size_t i(0); i < 3; i++)
{ {
ros_msg.position[i] = state.position[i]; ros_msg.position[i] = state.position[i];
ros_msg.velocity[i] = state.velocity[i]; ros_msg.velocity[i] = state.velocity[i];
} }
for (int i(0); i < 40; i++) for (std::size_t i(0); i < 40; i++)
{ {
ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
} }
for (int i(0); i < 20; i++) for (std::size_t i(0); i < 20; i++)
{ {
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
} }
@ -301,34 +239,85 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state
return ros_msg; return ros_msg;
} }
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg) unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state)
{ {
UNITREE_LEGGED_SDK::HighCmd cmd; unitree_legged_msgs::LowState ros_msg;
cmd.head[0] = 0xFE; for (std::size_t i(0); i < 2; i++)
cmd.head[1] = 0xEF; {
cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL; ros_msg.head[i] = state.head[i];
cmd.mode = 0; ros_msg.SN[i] = state.SN[i];
cmd.gaitType = 0; ros_msg.version[i] = state.version[i];
cmd.speedLevel = 0; }
cmd.footRaiseHeight = 0;
cmd.bodyHeight = 0;
cmd.euler[0] = 0;
cmd.euler[1] = 0;
cmd.euler[2] = 0;
cmd.velocity[0] = 0.0f;
cmd.velocity[1] = 0.0f;
cmd.yawSpeed = 0.0f;
cmd.reserve = 0;
cmd.velocity[0] = msg->linear.x; for (std::size_t i(0); i < 4; i++)
cmd.velocity[1] = msg->linear.y; {
cmd.yawSpeed = msg->angular.z; ros_msg.footForce[i] = state.footForce[i];
ros_msg.footForceEst[i] = state.footForceEst[i];
}
cmd.mode = 2; for (std::size_t i(0); i < 40; i++)
cmd.gaitType = 1; {
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
}
for (std::size_t i(0); i < 20; i++)
{
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
}
ros_msg.imu = state2rosMsg(state.imu);
ros_msg.bms = state2rosMsg(state.bms);
ros_msg.tick = state.tick;
ros_msg.reserve = state.reserve;
ros_msg.crc = state.crc;
return ros_msg;
}
UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd &msg)
{
UNITREE_LEGGED_SDK::LowCmd cmd;
for (std::size_t i(0); i < 2; i++)
{
cmd.head[i] = msg.head[i];
cmd.SN[i] = msg.SN[i];
cmd.version[i] = msg.version[i];
}
for (std::size_t i(0); i < 40; i++)
{
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
}
for (std::size_t i(0); i < 20; i++)
{
cmd.motorCmd[i] = rosMsg2Cmd(msg.motorCmd[i]);
}
cmd.bms = rosMsg2Cmd(msg.bms);
cmd.levelFlag = msg.levelFlag;
cmd.frameReserve = msg.frameReserve;
cmd.bandWidth = msg.bandWidth;
cmd.reserve = msg.reserve;
cmd.crc = msg.crc;
return cmd; return cmd;
} }
#endif // _CONVERT_H_ #endif // _CONVERT_H_

8
unitree_legged_real/launch/keyboard_control.launch
View File

@ -1,8 +0,0 @@
<launch>
<node pkg="unitree_legged_real" type="twist_sub" name="node_twist_sub" output="screen"/>
<node pkg="unitree_legged_real" type="control_via_keyboard" name="node_control_via_keyboard" output="screen"/>
</launch>

111
unitree_legged_real/src/exe/control_via_keyboard.cpp
View File

@ -1,111 +0,0 @@
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <termios.h>
int getch()
{
int ch;
struct termios oldt;
struct termios newt;
// Store old settings, and copy to new settings
tcgetattr(STDIN_FILENO, &oldt);
newt = oldt;
// Make required changes and apply the settings
newt.c_lflag &= ~(ICANON | ECHO);
newt.c_iflag |= IGNBRK;
newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF);
newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN);
newt.c_cc[VMIN] = 0;
newt.c_cc[VTIME] = 1;
tcsetattr(fileno(stdin), TCSANOW, &newt);
// Get the current character
ch = getchar();
// Reapply old settings
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
return ch;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "keyboard_input_node");
ros::NodeHandle nh;
ros::Rate loop_rate(500);
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
geometry_msgs::Twist twist;
long count = 0;
while (ros::ok())
{
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.linear.z = 0.0;
twist.angular.x = 0.0;
twist.angular.y = 0.0;
twist.angular.z = 0.0;
int ch = 0;
ch = getch();
printf("%ld\n", count++);
printf("ch = %d\n\n", ch);
switch (ch)
{
case 'q':
printf("already quit!\n");
return 0;
case 'w':
twist.linear.x = 0.5;
printf("move forward!\n");
break;
case 's':
twist.linear.x = -0.5;
printf("move backward!\n");
break;
case 'a':
twist.linear.y = 0.5;
printf("move left!\n");
break;
case 'd':
twist.linear.y = -0.5;
printf("move right!\n");
break;
case 'j':
twist.angular.z = 1.0;
printf("turn left!\n");
break;
case 'l':
twist.angular.z = -1.0;
printf("turn right!\n");
break;
default:
printf("Stop!\n");
break;
}
pub.publish(twist);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

90
unitree_legged_real/src/exe/example_position.cpp
View File

@ -6,6 +6,18 @@
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
unitree_legged_msgs::LowState low_state_ros;
void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &state)
{
static long count = 0;
ROS_INFO("lowStateCallback %ld", count++);
low_state_ros = *state;
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
ros::init(argc, argv, "example_postition_without_lcm"); ros::init(argc, argv, "example_postition_without_lcm");
@ -29,16 +41,17 @@ int main(int argc, char **argv)
unitree_legged_msgs::LowCmd low_cmd_ros; unitree_legged_msgs::LowCmd low_cmd_ros;
bool initiated_flag = false; // initiate need time // bool initiated_flag = false; // initiate need time
int count = 0; // int count = 0;
ros::Publisher pub = nh.advertise<unitree_legged_msgs::LowCmd>("low_cmd", 1); ros::Publisher pub = nh.advertise<unitree_legged_msgs::LowCmd>("low_cmd", 1);
ros::Subscriber sub = nh.subscribe("low_state", 1, lowStateCallback);
low_cmd_ros.head[0] = 0xFE; low_cmd_ros.head[0] = 0xFE;
low_cmd_ros.head[1] = 0xEF; low_cmd_ros.head[1] = 0xEF;
low_cmd_ros.levelFlag = LOWLEVEL; low_cmd_ros.levelFlag = LOWLEVEL;
for (int i = 0; i < 12; i++) for (std::size_t i = 0; i < 20; i++)
{ {
low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环 low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环
@ -51,37 +64,60 @@ int main(int argc, char **argv)
while (ros::ok()) while (ros::ok())
{ {
if (initiated_flag == true) printf("FR_joint_pos: %f %f %f\n", low_state_ros.motorState[FR_0].q, low_state_ros.motorState[FR_1].q, low_state_ros.motorState[FR_2].q);
motiontime += 2;
if(motiontime >= 0 && motiontime <= 10)
{ {
motiontime += 2; qInit[0] = low_state_ros.motorState[FR_0].q;
qInit[1] = low_state_ros.motorState[FR_1].q;
qInit[2] = low_state_ros.motorState[FR_2].q;
}
else if(motiontime > 10 && motiontime < 1000)
{
Kp[0] = 20.0;
Kp[1] = 20.0;
Kp[2] = 20.0;
Kd[0] = 2.0;
Kd[1] = 2.0;
Kd[2] = 2.0;
low_cmd_ros.motorCmd[FR_0].tau = -0.65f; const float interval = 800.0;
low_cmd_ros.motorCmd[FL_0].tau = +0.65f;
low_cmd_ros.motorCmd[RR_0].tau = -0.65f;
low_cmd_ros.motorCmd[RL_0].tau = +0.65f;
low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3); qDes[0] = (1 - (motiontime - 10) / interval) * qInit[0] + (motiontime - 10) / interval * sin_mid_q[0];
low_cmd_ros.motorCmd[FR_2].dq = 0.0; qDes[1] = (1 - (motiontime - 10) / interval) * qInit[1] + (motiontime - 10) / interval * sin_mid_q[1];
low_cmd_ros.motorCmd[FR_2].Kp = 5.0; qDes[2] = (1 - (motiontime - 10) / interval) * qInit[2] + (motiontime - 10) / interval * sin_mid_q[2];
low_cmd_ros.motorCmd[FR_2].Kd = 1.0; }
else if(motiontime >= 1000)
{
float period = 5.0;
low_cmd_ros.motorCmd[FR_0].q = 0.0; qDes[0] = sin_mid_q[0];
low_cmd_ros.motorCmd[FR_0].dq = 0.0; qDes[1] = sin_mid_q[1] + 0.6 * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0);
low_cmd_ros.motorCmd[FR_0].Kp = 5.0; qDes[2] = sin_mid_q[2] + (-0.9) * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0);
low_cmd_ros.motorCmd[FR_0].Kd = 1.0;
low_cmd_ros.motorCmd[FR_1].q = 0.0;
low_cmd_ros.motorCmd[FR_1].dq = 0.0;
low_cmd_ros.motorCmd[FR_1].Kp = 5.0;
low_cmd_ros.motorCmd[FR_1].Kd = 1.0;
} }
count++; low_cmd_ros.motorCmd[FR_0].tau = -4.0;
if (count > 10) low_cmd_ros.motorCmd[FR_0].Kp = Kp[0];
{ low_cmd_ros.motorCmd[FR_0].Kd = Kd[0];
count = 10; low_cmd_ros.motorCmd[FR_0].q = qDes[0];
initiated_flag = true; low_cmd_ros.motorCmd[FR_0].dq = 0.0;
}
low_cmd_ros.motorCmd[FR_1].tau = 0.0;
low_cmd_ros.motorCmd[FR_1].Kp = Kp[1];
low_cmd_ros.motorCmd[FR_1].Kd = Kd[1];
low_cmd_ros.motorCmd[FR_1].q = qDes[1];
low_cmd_ros.motorCmd[FR_1].dq = 0.0;
low_cmd_ros.motorCmd[FR_2].tau = 0.0;
low_cmd_ros.motorCmd[FR_2].Kp = Kp[2];
low_cmd_ros.motorCmd[FR_2].Kd = Kd[2];
low_cmd_ros.motorCmd[FR_2].q = qDes[2];
low_cmd_ros.motorCmd[FR_2].dq = 0.0;
pub.publish(low_cmd_ros); pub.publish(low_cmd_ros);

97
unitree_legged_real/src/exe/example_walk.cpp
View File

@ -6,6 +6,15 @@
using namespace UNITREE_LEGGED_SDK; using namespace UNITREE_LEGGED_SDK;
unitree_legged_msgs::HighState high_state_ros;
void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &state)
{
static long count = 0;
ROS_INFO("highStateCallback %ld", count++);
high_state_ros = *state;
}
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
ros::init(argc, argv, "example_walk_without_lcm"); ros::init(argc, argv, "example_walk_without_lcm");
@ -24,9 +33,11 @@ int main(int argc, char **argv)
unitree_legged_msgs::HighCmd high_cmd_ros; unitree_legged_msgs::HighCmd high_cmd_ros;
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000); ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
ros::Subscriber sub = nh.subscribe("high_state", 1000, highStateCallback);
while (ros::ok()) while (ros::ok())
{ {
printf("rpy: %f %f %f\n", high_state_ros.imu.rpy[0], high_state_ros.imu.rpy[1], high_state_ros.imu.rpy[2]);
motiontime += 2; motiontime += 2;
@ -46,89 +57,77 @@ int main(int argc, char **argv)
high_cmd_ros.yawSpeed = 0.0f; high_cmd_ros.yawSpeed = 0.0f;
high_cmd_ros.reserve = 0; high_cmd_ros.reserve = 0;
if (motiontime > 0 && motiontime < 1000) if (motiontime > 0 && motiontime < 2000)
{
high_cmd_ros.mode = 6;
}
else if(motiontime >= 2000 && motiontime < 3000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[0] = -0.3;
} }
if (motiontime > 1000 && motiontime < 2000) else if(motiontime >= 3000 && motiontime < 4000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[0] = 0.3; high_cmd_ros.euler[0] = 0.3;
} }
if (motiontime > 2000 && motiontime < 3000) else if(motiontime >= 4000 && motiontime < 6000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[1] = -0.2; high_cmd_ros.euler[0] = -0.3;
} }
if (motiontime > 3000 && motiontime < 4000) else if(motiontime >= 6000 && motiontime < 8000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[1] = 0.2; high_cmd_ros.euler[1] = 0.3;
} }
if (motiontime > 4000 && motiontime < 5000) else if(motiontime >= 8000 && motiontime < 10000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[2] = -0.2; high_cmd_ros.euler[1] = -0.3;
} }
if (motiontime > 5000 && motiontime < 6000) else if(motiontime >= 10000 && motiontime < 12000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.euler[2] = 0.2; high_cmd_ros.euler[2] = 0.3;
} }
if (motiontime > 6000 && motiontime < 7000) else if(motiontime >= 12000 && motiontime < 14000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = -0.2; high_cmd_ros.euler[2] = -0.3;
} }
if (motiontime > 7000 && motiontime < 8000) else if(motiontime >= 14000 && motiontime < 15000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = 0.1;
} }
if (motiontime > 8000 && motiontime < 9000) else if(motiontime >= 15000 && motiontime < 18000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = 0.0;
}
if (motiontime > 9000 && motiontime < 11000)
{
high_cmd_ros.mode = 5;
}
if (motiontime > 11000 && motiontime < 13000)
{
high_cmd_ros.mode = 6;
}
if (motiontime > 13000 && motiontime < 14000)
{
high_cmd_ros.mode = 0;
}
if (motiontime > 14000 && motiontime < 18000)
{ {
high_cmd_ros.mode = 2; high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 2; high_cmd_ros.velocity[0] = 0.3;
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1 high_cmd_ros.yawSpeed = 0.2;
high_cmd_ros.yawSpeed = 2;
high_cmd_ros.footRaiseHeight = 0.1;
// printf("walk\n");
} }
if (motiontime > 18000 && motiontime < 20000) else if(motiontime >= 18000 && motiontime < 21000)
{
high_cmd_ros.mode = 0;
high_cmd_ros.velocity[0] = 0;
}
if (motiontime > 20000 && motiontime < 24000)
{ {
high_cmd_ros.mode = 2; high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 1; high_cmd_ros.velocity[1] = -0.3;
high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1 high_cmd_ros.yawSpeed = -0.2;
high_cmd_ros.bodyHeight = 0.1;
// printf("walk\n");
} }
if (motiontime > 24000) else if(motiontime >= 21000 && motiontime < 22000)
{ {
high_cmd_ros.mode = 1; high_cmd_ros.mode = 1;
} }
else if(motiontime >= 22000 && motiontime < 25000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 3;
}
else if(motiontime >= 25000 && motiontime < 26000)
{
high_cmd_ros.mode = 1;
}
else
{
high_cmd_ros.mode = 0;
}
pub.publish(high_cmd_ros); pub.publish(high_cmd_ros);

18
unitree_legged_real/src/exe/ros_udp.cpp
View File

@ -27,7 +27,7 @@ public:
: :
// low_udp(LOWLEVEL), // low_udp(LOWLEVEL),
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007), low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)) high_udp(8090, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState))
{ {
high_udp.InitCmdData(high_cmd); high_udp.InitCmdData(high_cmd);
low_udp.InitCmdData(low_cmd); low_udp.InitCmdData(low_cmd);
@ -77,9 +77,9 @@ long low_count = 0;
void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg) void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
{ {
printf("highCmdCallback is running !\t%ld\n", ::high_count); printf("highCmdCallback is running !\t%ld\n", ::high_count++);
custom.high_cmd = rosMsg2Cmd(msg); custom.high_cmd = rosMsg2Cmd(*msg);
unitree_legged_msgs::HighState high_state_ros; unitree_legged_msgs::HighState high_state_ros;
@ -87,15 +87,15 @@ void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
pub_high.publish(high_state_ros); pub_high.publish(high_state_ros);
printf("highCmdCallback ending !\t%ld\n\n", ::high_count++); // printf("highCmdCallback ending !\t%ld\n\n", ::high_count++);
} }
void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg) void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
{ {
printf("lowCmdCallback is running !\t%ld\n", low_count); printf("lowCmdCallback is running !\t%ld\n", ::low_count++);
custom.low_cmd = rosMsg2Cmd(msg); custom.low_cmd = rosMsg2Cmd(*msg);
unitree_legged_msgs::LowState low_state_ros; unitree_legged_msgs::LowState low_state_ros;
@ -103,7 +103,7 @@ void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
pub_low.publish(low_state_ros); pub_low.publish(low_state_ros);
printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++); // printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++);
} }
int main(int argc, char **argv) int main(int argc, char **argv)
@ -123,6 +123,8 @@ int main(int argc, char **argv)
loop_udpSend.start(); loop_udpSend.start();
loop_udpRecv.start(); loop_udpRecv.start();
printf("LOWLEVEL is initialized\n");
ros::spin(); ros::spin();
// printf("low level runing!\n"); // printf("low level runing!\n");
@ -138,6 +140,8 @@ int main(int argc, char **argv)
loop_udpSend.start(); loop_udpSend.start();
loop_udpRecv.start(); loop_udpRecv.start();
printf("HIGHLEVEL is initialized\n");
ros::spin(); ros::spin();
// printf("high level runing!\n"); // printf("high level runing!\n");

39
unitree_legged_real/src/exe/state_sub.cpp
View File

@ -1,39 +0,0 @@
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include <unitree_legged_msgs/MotorCmd.h>
#include <unitree_legged_msgs/MotorState.h>
#include <unitree_legged_msgs/BmsCmd.h>
#include <unitree_legged_msgs/BmsState.h>
#include <unitree_legged_msgs/IMU.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include <ros/ros.h>
using namespace UNITREE_LEGGED_SDK;
void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &msg)
{
printf("yaw = %f\n", msg->imu.rpy[2]);
}
void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &msg)
{
printf("FR_2_pos = %f\n", msg->motorState[FR_2].q);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "node_high_state_sub");
ros::NodeHandle nh;
unitree_legged_msgs::HighState high_state_ros;
ros::Subscriber high_sub = nh.subscribe("high_state", 1, highStateCallback);
ros::Subscriber low_sub = nh.subscribe("low_state", 1, lowStateCallback);
ros::spin();
return 0;
}

112
unitree_legged_real/src/exe/twist_sub.cpp
View File

@ -1,112 +0,0 @@
#include <ros/ros.h>
#include <unitree_legged_msgs/HighCmd.h>
#include <unitree_legged_msgs/HighState.h>
#include <unitree_legged_msgs/LowCmd.h>
#include <unitree_legged_msgs/LowState.h>
#include "unitree_legged_sdk/unitree_legged_sdk.h"
#include "convert.h"
#include <chrono>
#include <pthread.h>
#include <geometry_msgs/Twist.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),
low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
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_cmd_vel;
ros::Publisher pub_high;
long cmd_vel_count = 0;
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg)
{
printf("cmdVelCallback is running!\t%ld\n", cmd_vel_count);
custom.high_cmd = rosMsg2Cmd(msg);
printf("cmd_x_vel = %f\n", custom.high_cmd.velocity[0]);
printf("cmd_y_vel = %f\n", custom.high_cmd.velocity[1]);
printf("cmd_yaw_vel = %f\n", custom.high_cmd.yawSpeed);
unitree_legged_msgs::HighState high_state_ros;
high_state_ros = state2rosMsg(custom.high_state);
pub_high.publish(high_state_ros);
printf("cmdVelCallback ending!\t%ld\n\n", cmd_vel_count++);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "twist_sub");
ros::NodeHandle nh;
pub_high = nh.advertise<unitree_legged_msgs::HighState>("high_state", 1);
sub_cmd_vel = nh.subscribe("cmd_vel", 1, cmdVelCallback);
LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom));
LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom));
loop_udpSend.start();
loop_udpRecv.start();
ros::spin();
return 0;
}