Compare commits

..

5 Commits

Author SHA1 Message Date
Agnel Wang b989870124 Update unitree_legged_sdk 2023-06-27 21:30:55 +08:00
Agnel Wang 0cb2c57018 fix bug 2023-06-27 21:23:33 +08:00
Agnel Wang f352667991 add submodule unitree_legged_sdk 2023-06-27 21:06:59 +08:00
Agnel Wang 817d60c4d9 move legged_sdk module to unitree_ros 2023-06-27 18:01:37 +08:00
Agnel Wang 1f6498dc5a add go1 unitree_legged_sdk submodule 2023-06-27 15:32:30 +08:00
20 changed files with 735 additions and 447 deletions

3
.gitmodules vendored Normal file
View File

@ -0,0 +1,3 @@
[submodule "unitree_legged_sdk"]
path = unitree_legged_sdk
url = https://github.com/unitreerobotics/unitree_legged_sdk.git

View File

@ -1,9 +1,9 @@
Packages Version: v3.8.4
Packages Version: v3.8.0
# 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 version is suitable for unitree_legged_sdk v3.8.4, namely Aliengo robot.
This version is suitable for unitree_legged_sdk v3.5.1, namely Go1 robot.
## Packages:
@ -12,24 +12,51 @@ Basic message function: `unitree_legged_msgs`
The interface between ROS and real robot: `unitree_legged_real`
## Environment
We recommand users to run this package in Ubuntu 20.04 and ROS neotic environment
We recommand users to run this package in Ubuntu 18.04 and ROS melodic environment
## Dependency
## Dependencies
* [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk/releases)
### Notice
The release [v3.8.4](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.8.4) only supports for robot: Aliengo.
The newest release [v3.8.0](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.8.0) only supports for robot: Go1.
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
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:
You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then:
```
cd ~/catkin_ws
catkin_make
```
# Run the package
You can control your real Aliengo robot from ROS by this package.
# 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`.
Before you run expamle program, please run
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
You can control your real Go1 robot from ROS by this package.
Before you run expamle program, please run command
```
roslaunch unitree_legged_real real.launch ctrl_level:=highlevel
@ -41,15 +68,25 @@ roslaunch unitree_legged_real real.launch ctrl_level:=lowlevel
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
Then, if you want to run high-level control mode, you can run example_walk node like this
```
rosrun unitree_legged_real example_walk
rosrun unitree_legged_real ros_example_walk
```
If you want to run low-level control mode, you can run example_position program node
If you want to run low-level control mode, you can run example_position program node like this
```
rosrun unitree_legged_real example_position
rosrun unitree_legged_real ros_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
mode in which you can do low-level control, finally make sure you hang the robot up before you run low-level control.
mode in which you can do joint-level control, finally make sure you hang the robot up before you run low-level control.

View File

@ -12,6 +12,8 @@ add_message_files(
FILES
MotorCmd.msg
MotorState.msg
BmsCmd.msg
BmsState.msg
Cartesian.msg
IMU.msg
LED.msg

View File

@ -0,0 +1,2 @@
uint8 off # off 0xA5
uint8[3] reserve

View File

@ -0,0 +1,9 @@
uint8 version_h
uint8 version_l
uint8 bms_status
uint8 SOC # SOC 0-100%
int32 current # mA
uint16 cycle
int8[2] BQ_NTC # x1 degrees centigrade
int8[2] MCU_NTC # x1 degrees centigrade
uint16[10] cell_vol # cell voltage mV

View File

@ -1,18 +1,23 @@
uint8[2] head
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint8 frameReserve
uint32[2] SN
uint32[2] version
uint16 bandWidth
uint8 mode
uint8 gaitType
uint8 speedLevel
float32 dFootRaiseHeight
float32 dBodyHeight
uint8 speedLevel
float32 footRaiseHeight
float32 bodyHeight
float32[2] position
float32[3] rpy
float32[3] euler
float32[2] velocity
float32 yawSpeed
float32 yawSpeed
BmsCmd bms
LED[4] led
uint8[40] wirelessRemote
uint32 reserve
uint32 crc

View File

@ -1,21 +1,28 @@
uint8[2] head
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint8 mode
uint8 frameReserve
uint32[2] SN
uint32[2] version
uint16 bandWidth
IMU imu
MotorState[20] motorState
BmsState bms
int16[4] footForce
int16[4] footForceEst
uint8 mode
float32 progress
uint8 gaitType
float32 footRaiseHeight
float32[3] position
float32 bodyHeight
float32[3] velocity
float32 yawSpeed
float32 yawSpeed
float32[4] rangeObstacle
Cartesian[4] footPosition2Body
Cartesian[4] footSpeed2Body
int16[4] footForce
uint8[40] wirelessRemote
uint32 reserve
uint32 crc
uint32 crc

View File

@ -1,12 +1,14 @@
uint8[2] head
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint8 frameReserve
uint32[2] SN
uint32[2] version
uint16 bandWidth
MotorCmd[20] motorCmd
LED[4] led
BmsCmd bms
uint8[40] wirelessRemote
uint32 reserve
uint32 crc
uint32 crc

View File

@ -1,22 +1,25 @@
uint8[2] head
uint8 levelFlag
uint16 commVersion
uint16 robotID
uint32 SN
uint8 bandWidth
uint8 frameReserve
uint32[2] SN
uint32[2] version
uint16 bandWidth
IMU imu
MotorState[20] motorState
BmsState bms
int16[4] footForce
int16[4] footForceEst
uint32 tick
int16[4] footForceEst
uint32 tick
uint8[40] wirelessRemote
uint32 reserve
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

View File

@ -1,12 +1,13 @@
cmake_minimum_required(VERSION 2.8.3)
project(unitree_legged_real)
add_compile_options(-std=c++11)
set(CMAKE_CXX_FLAGS "-O3 -fPIC -std=c++11")
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
unitree_legged_msgs
unitree_legged_sdk
)
catkin_package()
@ -18,31 +19,35 @@ else()
set(ARCH arm64)
endif()
link_directories(${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/lib/cpp/${ARCH})
set(EXTRA_LIBS -pthread libunitree_legged_sdk.so)
set(CMAKE_CXX_FLAGS "-O3 -fPIC")
set(EXTRA_LIBS -pthread ${unitree_legged_sdk_LIBRARIES})
include_directories(
include
${catkin_INCLUDE_DIRS}
${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/include
${unitree_legged_sdk_INCLUDE_DIRS}
)
add_executable(ros_example_walk src/exe/example_walk.cpp)
target_link_libraries(ros_example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(ros_example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(ros_example_position src/exe/example_position.cpp)
target_link_libraries(ros_example_position ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(ros_example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(example_walk src/exe/example_walk.cpp)
target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_executable(example_position src/exe/example_position.cpp)
target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES})
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)
target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES})
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})

View File

@ -6,56 +6,76 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
#ifndef _CONVERT_H_
#define _CONVERT_H_
#include "unitree_legged_msgs/Cartesian.h"
#include "unitree_legged_msgs/HighCmd.h"
#include "unitree_legged_msgs/HighState.h"
#include "unitree_legged_msgs/IMU.h"
#include "unitree_legged_msgs/LED.h"
#include "unitree_legged_msgs/LowCmd.h"
#include "unitree_legged_msgs/LowState.h"
#include "unitree_legged_msgs/MotorCmd.h"
#include "unitree_legged_msgs/MotorState.h"
#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"
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
{
unitree_legged_msgs::Cartesian ros_msg;
UNITREE_LEGGED_SDK::BmsCmd cmd;
ros_msg.x = state.x;
ros_msg.y = state.y;
ros_msg.z = state.z;
cmd.off = msg.off;
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)
ros_msg.quaternion[i] = state.quaternion[i];
for (std::size_t i(0); i < 3; ++i)
for (int i(0); i < 3; i++)
{
ros_msg.gyroscope[i] = state.gyroscope[i];
ros_msg.accelerometer[i] = state.accelerometer[i];
ros_msg.rpy[i] = state.rpy[i];
cmd.reserve[i] = msg.reserve[i];
}
ros_msg.temperature = state.temperature;
return ros_msg;
return cmd;
}
UNITREE_LEGGED_SDK::LED rosMsg2Cmd(const unitree_legged_msgs::LED &msg)
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
{
UNITREE_LEGGED_SDK::LED cmd;
cmd.r = msg.r;
cmd.g = msg.g;
cmd.b = msg.b;
UNITREE_LEGGED_SDK::HighCmd 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];
cmd.position[i] = msg->position[i];
cmd.velocity[i] = msg->velocity[i];
}
for (int i(0); i < 3; i++)
{
cmd.euler[i] = msg->euler[i];
}
for (int i(0); i < 4; i++)
{
cmd.led[i].r = msg->led[i].r;
cmd.led[i].g = msg->led[i].g;
cmd.led[i].b = msg->led[i].b;
}
for (int i(0); i < 40; i++)
{
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
}
cmd.levelFlag = msg->levelFlag;
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);
return cmd;
}
@ -71,13 +91,45 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
cmd.Kp = msg.Kp;
cmd.Kd = msg.Kd;
for (std::size_t i(0); i < 3; ++i)
for (int i(0); i < 3; i++)
{
cmd.reserve[i] = msg.reserve[i];
}
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)
{
@ -99,78 +151,48 @@ unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &sta
return ros_msg;
}
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd &msg)
unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
{
UNITREE_LEGGED_SDK::HighCmd cmd;
unitree_legged_msgs::IMU ros_msg;
cmd.levelFlag = msg.levelFlag;
cmd.commVersion = msg.commVersion;
cmd.robotID = msg.robotID;
cmd.SN = msg.SN;
cmd.bandWidth = msg.bandWidth;
cmd.mode = msg.mode;
cmd.gaitType = msg.gaitType;
cmd.speedLevel = msg.speedLevel;
cmd.dFootRaiseHeight = msg.dFootRaiseHeight;
cmd.dBodyHeight = msg.dBodyHeight;
for (std::size_t i(0); i < 2; ++i)
for (int i(0); i < 4; i++)
{
cmd.position[i] = msg.position[i];
cmd.velocity[i] = msg.velocity[i];
ros_msg.quaternion[i] = state.quaternion[i];
}
for (std::size_t i(0); i < 3; ++i)
cmd.rpy[i] = msg.rpy[i];
for (int i(0); i < 3; i++)
{
ros_msg.gyroscope[i] = state.gyroscope[i];
ros_msg.accelerometer[i] = state.accelerometer[i];
ros_msg.rpy[i] = state.rpy[i];
}
cmd.yawSpeed = msg.yawSpeed;
for (std::size_t i(0); i < 4; ++i)
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
ros_msg.temperature = state.temperature;
for (std::size_t i(0); i < 40; ++i)
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
cmd.reserve = msg.reserve;
cmd.crc = msg.crc;
return cmd;
return ros_msg;
}
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
{
unitree_legged_msgs::HighState ros_msg;
unitree_legged_msgs::BmsState ros_msg;
ros_msg.levelFlag = state.levelFlag;
ros_msg.commVersion = state.commVersion;
ros_msg.robotID = state.robotID;
ros_msg.SN = state.SN;
ros_msg.bandWidth = state.bandWidth;
ros_msg.mode = state.mode;
ros_msg.imu = state2rosMsg(state.imu);
for(std::size_t i(0); i < 3; ++i)
for (int i(0); i < 2; i++)
{
ros_msg.position[i] = state.position[i];
ros_msg.velocity[i] = state.velocity[i];
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
}
ros_msg.yawSpeed = state.yawSpeed;
for(std::size_t i(0); i < 4; ++i)
for (int i(0); i < 10; i++)
{
ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
ros_msg.footForce[i] = state.footForce[i];
ros_msg.cell_vol[i] = state.cell_vol[i];
}
for (std::size_t i(0); i < 40; ++i)
ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
ros_msg.reserve = state.reserve;
ros_msg.crc = state.crc;
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;
}
@ -179,69 +201,134 @@ unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state)
{
unitree_legged_msgs::LowState ros_msg;
ros_msg.levelFlag = state.levelFlag;
ros_msg.commVersion = state.commVersion;
ros_msg.robotID = state.robotID;
ros_msg.SN = state.SN;
ros_msg.bandWidth = state.bandWidth;
ros_msg.imu = state2rosMsg(state.imu);
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 (std::size_t i(0); i < 20; ++i)
ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
for (std::size_t i(0); i < 4; ++i)
for (int i(0); i < 4; i++)
{
ros_msg.footForce[i] = state.footForce[i];
ros_msg.footForceEst[i] = state.footForceEst[i];
}
ros_msg.tick = state.tick;
for (std::size_t i(0); i < 40; ++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_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd &msg)
unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
{
UNITREE_LEGGED_SDK::LowCmd cmd;
unitree_legged_msgs::Cartesian ros_msg;
cmd.levelFlag = msg.levelFlag;
cmd.commVersion = msg.commVersion;
cmd.robotID = msg.robotID;
cmd.SN = msg.SN;
cmd.bandWidth = msg.bandWidth;
ros_msg.x = state.x;
ros_msg.y = state.y;
ros_msg.z = state.z;
for (std::size_t i(0); i < 20; ++i)
cmd.motorCmd[i] = rosMsg2Cmd(msg.motorCmd[i]);
return ros_msg;
}
for(std::size_t i(0); i < 4; ++i)
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
{
unitree_legged_msgs::HighState ros_msg;
for (std::size_t i(0); i < 40; ++i)
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
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];
}
cmd.reserve = msg.reserve;
cmd.crc = msg.crc;
for (int i(0); i < 4; i++)
{
ros_msg.footForce[i] = state.footForce[i];
ros_msg.footForceEst[i] = state.footForceEst[i];
ros_msg.rangeObstacle[i] = state.rangeObstacle[i];
ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
}
for (int i(0); i < 3; i++)
{
ros_msg.position[i] = state.position[i];
ros_msg.velocity[i] = state.velocity[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.levelFlag = state.levelFlag;
ros_msg.frameReserve = state.frameReserve;
ros_msg.bandWidth = state.bandWidth;
ros_msg.mode = state.mode;
ros_msg.progress = state.progress;
ros_msg.gaitType = state.gaitType;
ros_msg.footRaiseHeight = state.footRaiseHeight;
ros_msg.bodyHeight = state.bodyHeight;
ros_msg.yawSpeed = state.yawSpeed;
ros_msg.reserve = state.reserve;
ros_msg.crc = state.crc;
return ros_msg;
}
UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg)
{
UNITREE_LEGGED_SDK::HighCmd cmd;
cmd.head[0] = 0xFE;
cmd.head[1] = 0xEF;
cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL;
cmd.mode = 0;
cmd.gaitType = 0;
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;
cmd.velocity[1] = msg->linear.y;
cmd.yawSpeed = msg->angular.z;
cmd.mode = 2;
cmd.gaitType = 1;
return cmd;
}
#endif // _CONVERT_H_

View File

@ -0,0 +1,8 @@
<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>

View File

@ -15,6 +15,7 @@
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<depend>unitree_legged_msgs</depend>
<depend>unitree_legged_sdk</depend>
<depend>eigen</depend>
</package>

View File

@ -0,0 +1,111 @@
#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;
}

View File

@ -6,26 +6,6 @@
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;
}
double jointLinearInterpolation(double initPos, double targetPos, double rate)
{
double p;
rate = std::min(std::max(rate, 0.0), 1.0);
p = initPos * (1 - rate) + targetPos * rate;
return p;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "example_postition_without_lcm");
@ -38,28 +18,27 @@ int main(int argc, char **argv)
ros::NodeHandle nh;
ros::Rate loop_rate(500);
long motiontime = 0;
int rate_count = 0;
int sin_count = 0;
float qInit[3] = {0};
float qDes[3] = {0};
float sin_mid_q[3] = {0.0, 1.2, -2.0};
float Kp[3] = {0};
float Kd[3] = {0};
double time_consume = 0;
int rate_count = 0;
int sin_count = 0;
int motiontime = 0;
float dt = 0.002;
unitree_legged_msgs::LowCmd low_cmd_ros;
// bool initiated_flag = false; // initiate need time
// int count = 0;
bool initiated_flag = false; // initiate need time
int count = 0;
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[1] = 0xEF;
low_cmd_ros.levelFlag = LOWLEVEL;
for (std::size_t i = 0; i < 20; ++i)
for (int i = 0; i < 12; i++)
{
low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode
low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环
@ -72,80 +51,37 @@ int main(int argc, char **argv)
while (ros::ok())
{
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;
low_cmd_ros.motorCmd[FR_0].tau = -1.6f;
if (motiontime >= 0)
if (initiated_flag == true)
{
// first, get record initial position
// if( motiontime >= 100 && motiontime < 500){
if (motiontime >= 0 && motiontime < 10)
{
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;
}
// second, move to the origin point of a sine movement with Kp Kd
// if( motiontime >= 500 && motiontime < 1500){
if (motiontime >= 10 && motiontime < 400)
{
rate_count++;
double rate = rate_count / 200.0; // needs count to 200
// Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0;
// Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0;
Kp[0] = 20.0;
Kp[1] = 20.0;
Kp[2] = 20.0;
Kd[0] = 2.0;
Kd[1] = 2.0;
Kd[2] = 2.0;
motiontime += 2;
qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
}
double sin_joint1, sin_joint2;
// last, do sine wave
float freq_Hz = 1;
// float freq_Hz = 5;
float freq_rad = freq_Hz * 2 * M_PI;
float t = dt * sin_count;
if (motiontime >= 400)
{
sin_count++;
// sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0);
// sin_joint2 = -0.9 * sin(3*M_PI*sin_count/1000.0);
sin_joint1 = 0.6 * sin(t * freq_rad);
sin_joint2 = -0.9 * sin(t * freq_rad);
qDes[0] = sin_mid_q[0];
qDes[1] = sin_mid_q[1] + sin_joint1;
qDes[2] = sin_mid_q[2] + sin_joint2;
// qDes[2] = sin_mid_q[2];
}
low_cmd_ros.motorCmd[FR_0].tau = -0.65f;
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_0].q = qDes[0];
low_cmd_ros.motorCmd[FR_0].dq = 0;
low_cmd_ros.motorCmd[FR_0].Kp = Kp[0];
low_cmd_ros.motorCmd[FR_0].Kd = Kd[0];
low_cmd_ros.motorCmd[FR_0].tau = -1.6f;
low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3);
low_cmd_ros.motorCmd[FR_2].dq = 0.0;
low_cmd_ros.motorCmd[FR_2].Kp = 5.0;
low_cmd_ros.motorCmd[FR_2].Kd = 1.0;
low_cmd_ros.motorCmd[FR_1].q = qDes[1];
low_cmd_ros.motorCmd[FR_1].dq = 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].tau = 0.0f;
low_cmd_ros.motorCmd[FR_0].q = 0.0;
low_cmd_ros.motorCmd[FR_0].dq = 0.0;
low_cmd_ros.motorCmd[FR_0].Kp = 5.0;
low_cmd_ros.motorCmd[FR_0].Kd = 1.0;
low_cmd_ros.motorCmd[FR_2].q = qDes[2];
low_cmd_ros.motorCmd[FR_2].dq = 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].tau = 0.0f;
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++;
if (count > 10)
{
count = 10;
initiated_flag = true;
}
pub.publish(low_cmd_ros);
@ -154,4 +90,4 @@ int main(int argc, char **argv)
}
return 0;
}
}

View File

@ -6,15 +6,6 @@
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)
{
ros::init(argc, argv, "example_walk_without_lcm");
@ -33,176 +24,111 @@ int main(int argc, char **argv)
unitree_legged_msgs::HighCmd high_cmd_ros;
ros::Publisher pub = nh.advertise<unitree_legged_msgs::HighCmd>("high_cmd", 1000);
ros::Subscriber sub = nh.subscribe("high_state", 1000, highStateCallback);
while (ros::ok())
{
std::cout << "motiontime:\t" << motiontime << " " << high_state_ros.imu.rpy[2] << "\n";
motiontime += 2;
high_cmd_ros.head[0] = 0xFE;
high_cmd_ros.head[1] = 0xEF;
high_cmd_ros.levelFlag = HIGHLEVEL;
high_cmd_ros.mode = 0;
high_cmd_ros.gaitType = 0;
high_cmd_ros.speedLevel = 0;
high_cmd_ros.footRaiseHeight = 0;
high_cmd_ros.bodyHeight = 0;
high_cmd_ros.euler[0] = 0;
high_cmd_ros.euler[1] = 0;
high_cmd_ros.euler[2] = 0;
high_cmd_ros.velocity[0] = 0.0f;
high_cmd_ros.velocity[1] = 0.0f;
high_cmd_ros.position[0] = 0.0f;
high_cmd_ros.position[1] = 0.0f;
high_cmd_ros.yawSpeed = 0.0f;
high_cmd_ros.reserve = 0;
high_cmd_ros.mode = 0;
high_cmd_ros.rpy[0] = 0;
high_cmd_ros.rpy[1] = 0;
high_cmd_ros.rpy[2] = 0;
high_cmd_ros.gaitType = 0;
high_cmd_ros.dBodyHeight = 0;
high_cmd_ros.dFootRaiseHeight = 0;
if (motiontime == 2)
{
std::cout<<"begin sending commands."<<std::endl;
}
if (motiontime>1000 && motiontime <2000)
{
high_cmd_ros.mode = 1; // mode 1: force stand, control robot orientation and height
high_cmd_ros.rpy[0] = 0.7;
}
if (motiontime>2000 && motiontime <3000)
if (motiontime > 0 && motiontime < 1000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.rpy[0] = 0.;
high_cmd_ros.euler[0] = -0.3;
}
if (motiontime>3000 && motiontime <4000)
if (motiontime > 1000 && motiontime < 2000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.rpy[1] = 0.4;
high_cmd_ros.euler[0] = 0.3;
}
if (motiontime>4000 && motiontime <5000)
if (motiontime > 2000 && motiontime < 3000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.rpy[1] = 0.;
high_cmd_ros.euler[1] = -0.2;
}
if (motiontime>5000 && motiontime <6000)
if (motiontime > 3000 && motiontime < 4000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.rpy[2] = 0.5;
high_cmd_ros.euler[1] = 0.2;
}
if (motiontime>6000 && motiontime <7000)
if (motiontime > 4000 && motiontime < 5000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.rpy[2] = 0;
high_cmd_ros.euler[2] = -0.2;
}
if (motiontime>8000 && motiontime <9000)
if (motiontime > 5000 && motiontime < 6000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.dBodyHeight = 0.05;
high_cmd_ros.euler[2] = 0.2;
}
if (motiontime>9000 && motiontime <10000)
if (motiontime > 6000 && motiontime < 7000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.dBodyHeight = -0.15;
high_cmd_ros.bodyHeight = -0.2;
}
if (motiontime>10000 && motiontime <11000)
if (motiontime > 7000 && motiontime < 8000)
{
high_cmd_ros.mode = 1;
high_cmd_ros.dBodyHeight = 0.;
high_cmd_ros.bodyHeight = 0.1;
}
if (motiontime>11000 && motiontime <13000)
if (motiontime > 8000 && motiontime < 9000)
{
high_cmd_ros.mode = 2; // mode 2: following tareget velocity in body frame
high_cmd_ros.gaitType = 0; // trot gait
high_cmd_ros.velocity[0] = 0.2;
high_cmd_ros.velocity[1] = 0;
high_cmd_ros.yawSpeed = 0.3;
high_cmd_ros.mode = 1;
high_cmd_ros.bodyHeight = 0.0;
}
if (motiontime>13000 && motiontime <15000)
if (motiontime > 9000 && motiontime < 11000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 0;
high_cmd_ros.velocity[0] = -0.2;
high_cmd_ros.velocity[1] = 0;
high_cmd_ros.yawSpeed = -0.1;
high_cmd_ros.dFootRaiseHeight = 0.1; // increase swing height
high_cmd_ros.mode = 5;
}
if (motiontime> 15000 && motiontime <17000)
if (motiontime > 11000 && motiontime < 13000)
{
high_cmd_ros.mode = 0; // normal trot of gaitType 0 will stop automatically
high_cmd_ros.mode = 6;
}
if (motiontime>17000 && motiontime <23000)
if (motiontime > 13000 && motiontime < 14000)
{
high_cmd_ros.mode = 3; // mode 3: walk to target position in ground frame
high_cmd_ros.gaitType = 0;
high_cmd_ros.speedLevel = 0; // adjust speedlevel
high_cmd_ros.position[0] = 1;
high_cmd_ros.position[1] = 0;
high_cmd_ros.rpy[2] = 0;
high_cmd_ros.dBodyHeight = -0.05; // lower body height
high_cmd_ros.mode = 0;
}
if (motiontime> 23000 && motiontime <25000)
{
high_cmd_ros.mode = 0; // normal trot of gaitType 0 will stop automatically
}
if (motiontime>25000 && motiontime <32000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 2; // stairs walking gait
high_cmd_ros.velocity[0] = 0.1; // avoid using higher forward speed than 0.37m/s for stair climbing gait
high_cmd_ros.velocity[1] = 0.;
high_cmd_ros.yawSpeed = 0;
}
if (motiontime>32000 && motiontime <40000)
if (motiontime > 14000 && motiontime < 18000)
{
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 2;
high_cmd_ros.velocity[0] = -0.1; // avoid using higher backward speed than -0.2m/s for stair climbing gait
high_cmd_ros.velocity[1] = 0.;
high_cmd_ros.yawSpeed = 0;
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1
high_cmd_ros.yawSpeed = 2;
high_cmd_ros.footRaiseHeight = 0.1;
// printf("walk\n");
}
if (motiontime>40000 && motiontime <42000)
if (motiontime > 18000 && motiontime < 20000)
{
high_cmd_ros.mode = 1; // stairs walking gait will not automatically stop. change mode to force stand.
high_cmd_ros.mode = 0;
high_cmd_ros.velocity[0] = 0;
}
if (motiontime>42000 && motiontime <44000)
if (motiontime > 20000 && motiontime < 24000)
{
high_cmd_ros.mode = 5; // stand down
high_cmd_ros.mode = 2;
high_cmd_ros.gaitType = 1;
high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1
high_cmd_ros.bodyHeight = 0.1;
// printf("walk\n");
}
if (motiontime>44000 && motiontime <46000)
if (motiontime > 24000)
{
high_cmd_ros.mode = 6; // stand up
high_cmd_ros.mode = 1;
}
if (motiontime>46000 && motiontime <48000)
{
high_cmd_ros.mode = 5; // stand down
}
if (motiontime>48000 && motiontime <51000)
{
high_cmd_ros.mode = 7; // damping mode
}
if (motiontime>51000 && motiontime <53000)
{
high_cmd_ros.mode = 5; // stand down
}
if (motiontime>53000 && motiontime <55000)
{
high_cmd_ros.mode = 6; // stand up
}
if (motiontime>55000 && motiontime <57000)
{
high_cmd_ros.mode = 1; // force stand
}
if (motiontime>57000 && motiontime <65000)
{
high_cmd_ros.mode = 3; // mode 3: walk to target position in ground frame
high_cmd_ros.gaitType = 0;
high_cmd_ros.speedLevel = 0; // adjust speedlevel
high_cmd_ros.position[0] = 0;
high_cmd_ros.position[1] = 0;
high_cmd_ros.rpy[2] = 0;
high_cmd_ros.dBodyHeight = 0;
}
pub.publish(high_cmd_ros);

View File

@ -10,10 +10,6 @@
#include <geometry_msgs/Twist.h>
using namespace UNITREE_LEGGED_SDK;
const int LOW_CMD_LENGTH = 610;
const int LOW_STATE_LENGTH = 771;
class Custom
{
public:
@ -30,8 +26,8 @@ public:
Custom()
:
// low_udp(LOWLEVEL),
low_udp(8082, "192.168.123.10", 8007, LOW_CMD_LENGTH, LOW_STATE_LENGTH),
high_udp(8081, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState))
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);
@ -81,25 +77,25 @@ long low_count = 0;
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;
high_state_ros = state2rosMsg(custom.high_state);
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)
{
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;
@ -107,7 +103,7 @@ void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg)
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)
@ -127,8 +123,6 @@ int main(int argc, char **argv)
loop_udpSend.start();
loop_udpRecv.start();
printf("LOWLEVEL is initialized\n");
ros::spin();
// printf("low level runing!\n");
@ -144,8 +138,6 @@ int main(int argc, char **argv)
loop_udpSend.start();
loop_udpRecv.start();
printf("HIGHLEVEL is initialized\n");
ros::spin();
// printf("high level runing!\n");

View File

@ -0,0 +1,39 @@
#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;
}

View File

@ -0,0 +1,112 @@
#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;
}

1
unitree_legged_sdk Submodule

@ -0,0 +1 @@
Subproject commit acc36dfd48755f5a9889ef3cc9ee31604ebfe41c