v3.8.3 support B1
This commit is contained in:
LuoBin
parent
c54489e721
commit
e3e96db914
59
README.md
59
README.md
|
|
@ -1,9 +1,9 @@
|
|||
Packages Version: v3.8.0
|
||||
Packages Version: v3.8.3
|
||||
|
||||
# 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.5.1, namely Go1 robot.
|
||||
This version is suitable for unitree_legged_sdk v3.8.3, namely B1 robot.
|
||||
|
||||
## Packages:
|
||||
|
||||
|
|
@ -12,51 +12,24 @@ 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 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)
|
||||
### Notice
|
||||
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.
|
||||
The release [v3.8.3](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.8.3) only supports for robot: B1.
|
||||
|
||||
# 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
|
||||
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
|
||||
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
|
||||
|
|
@ -68,25 +41,15 @@ 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 like this
|
||||
Then, if you want to run high-level control mode, you can run example_walk node
|
||||
```
|
||||
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
|
||||
```
|
||||
|
||||
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 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.
|
||||
|
|
|
|||
|
|
@ -4,6 +4,6 @@ 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
|
||||
int8[8] BQ_NTC # x1 degrees centigrade
|
||||
int8[8] MCU_NTC # x1 degrees centigrade
|
||||
uint16[30] cell_vol # cell voltage mV
|
||||
|
|
@ -15,6 +15,8 @@ float32[2] position
|
|||
float32[3] euler
|
||||
float32[2] velocity
|
||||
float32 yawSpeed
|
||||
float32[2] dComXy
|
||||
float32[2] dstandFootXy
|
||||
BmsCmd bms
|
||||
LED[4] led
|
||||
uint8[40] wirelessRemote
|
||||
|
|
|
|||
|
|
@ -16,10 +16,5 @@ 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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ else()
|
|||
set(ARCH arm64)
|
||||
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)
|
||||
|
||||
|
|
@ -27,16 +27,11 @@ set(CMAKE_CXX_FLAGS "-O3 -fPIC")
|
|||
include_directories(
|
||||
include
|
||||
${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)
|
||||
target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
|
||||
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})
|
||||
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})
|
||||
|
||||
|
|
|
|||
|
|
@ -6,18 +6,20 @@ 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/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_msgs/BmsCmd.h"
|
||||
#include "unitree_legged_msgs/BmsState.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_sdk/unitree_legged_sdk.h"
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include "ros/ros.h"
|
||||
|
||||
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;
|
||||
|
||||
for (int i(0); i < 3; i++)
|
||||
{
|
||||
for (std::size_t i(0); i < 3; i++)
|
||||
cmd.reserve[i] = msg.reserve[i];
|
||||
}
|
||||
|
||||
|
||||
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];
|
||||
cmd.SN[i] = msg->SN[i];
|
||||
cmd.version[i] = msg->version[i];
|
||||
cmd.position[i] = msg->position[i];
|
||||
cmd.velocity[i] = msg->velocity[i];
|
||||
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
|
||||
ros_msg.MCU_NTC[i] = state.MCU_NTC[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;
|
||||
cmd.led[i].g = msg->led[i].g;
|
||||
cmd.led[i].b = msg->led[i].b;
|
||||
ros_msg.gyroscope[i] = state.gyroscope[i];
|
||||
ros_msg.accelerometer[i] = state.accelerometer[i];
|
||||
ros_msg.rpy[i] = state.rpy[i];
|
||||
}
|
||||
|
||||
for (int i(0); i < 40; i++)
|
||||
{
|
||||
cmd.wirelessRemote[i] = msg->wirelessRemote[i];
|
||||
}
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
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;
|
||||
return ros_msg;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -91,7 +111,7 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
|
|||
cmd.Kp = msg.Kp;
|
||||
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];
|
||||
}
|
||||
|
|
@ -99,37 +119,6 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg
|
|||
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)
|
||||
{
|
||||
|
|
@ -151,113 +140,62 @@ unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &sta
|
|||
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++)
|
||||
{
|
||||
ros_msg.gyroscope[i] = state.gyroscope[i];
|
||||
ros_msg.accelerometer[i] = state.accelerometer[i];
|
||||
ros_msg.rpy[i] = state.rpy[i];
|
||||
}
|
||||
for (std::size_t i(0); i < 3; i++)
|
||||
cmd.euler[i] = msg.euler[i];
|
||||
|
||||
ros_msg.temperature = state.temperature;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
for (std::size_t i(0); i < 4; i++)
|
||||
cmd.led[i] = rosMsg2Cmd(msg.led[i]);
|
||||
|
||||
unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
|
||||
{
|
||||
unitree_legged_msgs::BmsState ros_msg;
|
||||
|
||||
for (int i(0); i < 2; i++)
|
||||
{
|
||||
ros_msg.BQ_NTC[i] = state.BQ_NTC[i];
|
||||
ros_msg.MCU_NTC[i] = state.MCU_NTC[i];
|
||||
}
|
||||
for (std::size_t i(0); i < 40; i++)
|
||||
cmd.wirelessRemote[i] = msg.wirelessRemote[i];
|
||||
|
||||
for (int i(0); i < 10; 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;
|
||||
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;
|
||||
|
||||
return ros_msg;
|
||||
}
|
||||
cmd.bms = rosMsg2Cmd(msg.bms);
|
||||
|
||||
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;
|
||||
return cmd;
|
||||
}
|
||||
|
||||
unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
|
||||
{
|
||||
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.SN[i] = state.SN[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.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]);
|
||||
}
|
||||
|
||||
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.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];
|
||||
}
|
||||
|
||||
for (int i(0); i < 20; i++)
|
||||
for (std::size_t i(0); i < 20; 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;
|
||||
}
|
||||
|
||||
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;
|
||||
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;
|
||||
for (std::size_t 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.velocity[0] = msg->linear.x;
|
||||
cmd.velocity[1] = msg->linear.y;
|
||||
cmd.yawSpeed = msg->angular.z;
|
||||
for (std::size_t i(0); i < 4; i++)
|
||||
{
|
||||
ros_msg.footForce[i] = state.footForce[i];
|
||||
ros_msg.footForceEst[i] = state.footForceEst[i];
|
||||
}
|
||||
|
||||
cmd.mode = 2;
|
||||
cmd.gaitType = 1;
|
||||
for (std::size_t i(0); i < 40; i++)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // _CONVERT_H_
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -6,6 +6,18 @@
|
|||
|
||||
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)
|
||||
{
|
||||
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;
|
||||
|
||||
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 (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].q = PosStopF; // 禁止位置环
|
||||
|
|
@ -51,37 +64,60 @@ int main(int argc, char **argv)
|
|||
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;
|
||||
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;
|
||||
const float interval = 800.0;
|
||||
|
||||
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;
|
||||
qDes[0] = (1 - (motiontime - 10) / interval) * qInit[0] + (motiontime - 10) / interval * sin_mid_q[0];
|
||||
qDes[1] = (1 - (motiontime - 10) / interval) * qInit[1] + (motiontime - 10) / interval * sin_mid_q[1];
|
||||
qDes[2] = (1 - (motiontime - 10) / interval) * qInit[2] + (motiontime - 10) / interval * sin_mid_q[2];
|
||||
}
|
||||
else if(motiontime >= 1000)
|
||||
{
|
||||
float period = 5.0;
|
||||
|
||||
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;
|
||||
qDes[0] = sin_mid_q[0];
|
||||
qDes[1] = sin_mid_q[1] + 0.6 * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0);
|
||||
qDes[2] = sin_mid_q[2] + (-0.9) * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.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++;
|
||||
if (count > 10)
|
||||
{
|
||||
count = 10;
|
||||
initiated_flag = true;
|
||||
}
|
||||
low_cmd_ros.motorCmd[FR_0].tau = -4.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].q = qDes[0];
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,15 @@
|
|||
|
||||
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");
|
||||
|
|
@ -24,9 +33,11 @@ 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())
|
||||
{
|
||||
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;
|
||||
|
||||
|
|
@ -46,89 +57,77 @@ int main(int argc, char **argv)
|
|||
high_cmd_ros.yawSpeed = 0.0f;
|
||||
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.euler[0] = -0.3;
|
||||
}
|
||||
if (motiontime > 1000 && motiontime < 2000)
|
||||
else if(motiontime >= 3000 && motiontime < 4000)
|
||||
{
|
||||
high_cmd_ros.mode = 1;
|
||||
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.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.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.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.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.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.bodyHeight = 0.1;
|
||||
}
|
||||
if (motiontime > 8000 && motiontime < 9000)
|
||||
{
|
||||
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)
|
||||
else if(motiontime >= 15000 && motiontime < 18000)
|
||||
{
|
||||
high_cmd_ros.mode = 2;
|
||||
high_cmd_ros.gaitType = 2;
|
||||
high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1
|
||||
high_cmd_ros.yawSpeed = 2;
|
||||
high_cmd_ros.footRaiseHeight = 0.1;
|
||||
// printf("walk\n");
|
||||
high_cmd_ros.velocity[0] = 0.3;
|
||||
high_cmd_ros.yawSpeed = 0.2;
|
||||
}
|
||||
if (motiontime > 18000 && motiontime < 20000)
|
||||
{
|
||||
high_cmd_ros.mode = 0;
|
||||
high_cmd_ros.velocity[0] = 0;
|
||||
}
|
||||
if (motiontime > 20000 && motiontime < 24000)
|
||||
else if(motiontime >= 18000 && motiontime < 21000)
|
||||
{
|
||||
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");
|
||||
high_cmd_ros.velocity[1] = -0.3;
|
||||
high_cmd_ros.yawSpeed = -0.2;
|
||||
}
|
||||
if (motiontime > 24000)
|
||||
else if(motiontime >= 21000 && motiontime < 22000)
|
||||
{
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -27,7 +27,7 @@ public:
|
|||
:
|
||||
// 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(8090, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState))
|
||||
{
|
||||
high_udp.InitCmdData(high_cmd);
|
||||
low_udp.InitCmdData(low_cmd);
|
||||
|
|
@ -77,9 +77,9 @@ 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;
|
||||
|
||||
|
|
@ -87,15 +87,15 @@ void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
|
|||
|
||||
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;
|
||||
|
||||
|
|
@ -103,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)
|
||||
|
|
@ -123,6 +123,8 @@ int main(int argc, char **argv)
|
|||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
printf("LOWLEVEL is initialized\n");
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("low level runing!\n");
|
||||
|
|
@ -138,6 +140,8 @@ int main(int argc, char **argv)
|
|||
loop_udpSend.start();
|
||||
loop_udpRecv.start();
|
||||
|
||||
printf("HIGHLEVEL is initialized\n");
|
||||
|
||||
ros::spin();
|
||||
|
||||
// printf("high level runing!\n");
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue