Update unitree_legged_sdk

.gitmodules

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

README.md

@@ -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
+# Setup the net connection
-You can control your real Aliengo robot from ROS by this package.
+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.

unitree_legged_msgs/CMakeLists.txt

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

unitree_legged_msgs/msg/BmsCmd.msg

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

unitree_legged_msgs/msg/BmsState.msg

@@ -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

unitree_legged_msgs/msg/HighCmd.msg

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

unitree_legged_msgs/msg/HighState.msg

@@ -1,21 +1,28 @@
+
+uint8[2] head
 uint8 levelFlag
-uint16 commVersion
+uint8 frameReserve
-uint16 robotID
+
-uint32 SN
+uint32[2] SN
-uint8 bandWidth
+uint32[2] version
-uint8 mode
+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[4] rangeObstacle
 Cartesian[4] footPosition2Body 
 Cartesian[4] footSpeed2Body	
-int16[4] footForce
 uint8[40] wirelessRemote
 uint32 reserve
+
 uint32 crc
-
-
-
-
-

unitree_legged_msgs/msg/LowCmd.msg

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

unitree_legged_msgs/msg/LowState.msg

@@ -1,10 +1,14 @@
+
+uint8[2] head
 uint8 levelFlag
-uint16 commVersion
+uint8 frameReserve
-uint16 robotID
+
-uint32 SN
+uint32[2] SN
-uint8 bandWidth
+uint32[2] version
+uint16 bandWidth
 IMU imu
 MotorState[20] motorState
+BmsState bms
 int16[4] footForce	
 int16[4] footForceEst	
 uint32 tick						
@@ -12,11 +16,10 @@ 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
-
 

unitree_legged_real/CMakeLists.txt

@@ -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 ${unitree_legged_sdk_LIBRARIES})
-
-set(EXTRA_LIBS -pthread libunitree_legged_sdk.so)
-
-set(CMAKE_CXX_FLAGS "-O3 -fPIC")
 
 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)
+add_executable(state_sub src/exe/state_sub.cpp)
-target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES})
+target_link_libraries(state_sub ${EXTRA_LIBS} ${catkin_LIBRARIES})
-add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(state_sub ${${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(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})
 

unitree_legged_real/include/convert.h

@@ -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/LowCmd.h>
-#include "unitree_legged_msgs/HighCmd.h"
+#include <unitree_legged_msgs/LowState.h>
-#include "unitree_legged_msgs/HighState.h"
+#include <unitree_legged_msgs/HighCmd.h>
-#include "unitree_legged_msgs/IMU.h"
+#include <unitree_legged_msgs/HighState.h>
-#include "unitree_legged_msgs/LED.h"
+#include <unitree_legged_msgs/MotorCmd.h>
-#include "unitree_legged_msgs/LowCmd.h"
+#include <unitree_legged_msgs/MotorState.h>
-#include "unitree_legged_msgs/LowState.h"
+#include <unitree_legged_msgs/BmsCmd.h>
-#include "unitree_legged_msgs/MotorCmd.h"
+#include <unitree_legged_msgs/BmsState.h>
-#include "unitree_legged_msgs/MotorState.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_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg)
-unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state)
 {
-    unitree_legged_msgs::Cartesian ros_msg;
+    UNITREE_LEGGED_SDK::BmsCmd cmd;
 
-    ros_msg.x = state.x;
+    cmd.off = msg.off;
-    ros_msg.y = state.y;
-    ros_msg.z = state.z;
 
-    return ros_msg;
+    for (int i(0); i < 3; i++)
+    {
+        cmd.reserve[i] = msg.reserve[i];
     }
 
-unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state)
+    return cmd;
-{
-    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)
-    {
-        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;
+UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg)
+{
+    UNITREE_LEGGED_SDK::HighCmd cmd;
 
-    return ros_msg;
+    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];
     }
 
-UNITREE_LEGGED_SDK::LED rosMsg2Cmd(const unitree_legged_msgs::LED &msg)
+    for (int i(0); i < 3; i++)
     {
-    UNITREE_LEGGED_SDK::LED cmd;
+        cmd.euler[i] = msg->euler[i];
-    cmd.r = msg.r;
+    }
-    cmd.g = msg.g;
+
-    cmd.b = msg.b;
+    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;
+    for (int i(0); i < 4; i++)
-    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)
     {
-        cmd.position[i] = msg.position[i];
+        ros_msg.quaternion[i] = state.quaternion[i];
-        cmd.velocity[i] = msg.velocity[i];
     }
 
-    for (std::size_t i(0); i < 3; ++i)
+    for (int i(0); i < 3; i++)
-        cmd.rpy[i] = msg.rpy[i];
+    {
-
+        ros_msg.gyroscope[i] = state.gyroscope[i];
-    cmd.yawSpeed = msg.yawSpeed;
+        ros_msg.accelerometer[i] = state.accelerometer[i];
-    
+        ros_msg.rpy[i] = state.rpy[i];
-    for (std::size_t i(0); i < 4; ++i)
-        cmd.led[i] = rosMsg2Cmd(msg.led[i]);
-    
-
-    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;
     }
 
-unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
+    ros_msg.temperature = state.temperature;
-{
-    unitree_legged_msgs::HighState ros_msg;
 
-    ros_msg.levelFlag = state.levelFlag;
+    return ros_msg;
-    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)
-    {
-        ros_msg.position[i] = state.position[i];
-        ros_msg.velocity[i] = state.velocity[i];
 }
 
-    ros_msg.yawSpeed = state.yawSpeed;
+unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state)
-
-    for(std::size_t i(0); i < 4; ++i)
 {
-        ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]);
+    unitree_legged_msgs::BmsState ros_msg;
-        ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]);
+
-        ros_msg.footForce[i] = state.footForce[i];
+    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)
+    for (int i(0); i < 10; i++)
-        ros_msg.wirelessRemote[i] = state.wirelessRemote[i];
+    {
-    
+        ros_msg.cell_vol[i] = state.cell_vol[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;
+    for (int i(0); i < 2; i++)
-    ros_msg.commVersion = state.commVersion;
+    {
-    ros_msg.robotID = state.robotID;
+        ros_msg.head[i] = state.head[i];
-    ros_msg.SN = state.SN;
+        ros_msg.SN[i] = state.SN[i];
-    ros_msg.bandWidth = state.bandWidth;
+        ros_msg.version[i] = state.version[i];
-    ros_msg.imu = state2rosMsg(state.imu);
+    }
 
-    for (std::size_t i(0); i < 20; ++i)
+    for (int i(0); i < 4; i++)
-        ros_msg.motorState[i] = state2rosMsg(state.motorState[i]);
-
-    for (std::size_t 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 (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++)
+    {
+        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_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd &msg)
 {
-    UNITREE_LEGGED_SDK::LowCmd cmd;
+    unitree_legged_msgs::Cartesian ros_msg;
 
-    cmd.levelFlag = msg.levelFlag;
+    ros_msg.x = state.x;
-    cmd.commVersion = msg.commVersion;
+    ros_msg.y = state.y;
-    cmd.robotID = msg.robotID;
+    ros_msg.z = state.z;
-    cmd.SN = msg.SN;
-    cmd.bandWidth = msg.bandWidth;
 
-    for (std::size_t i(0); i < 20; ++i)
+    return ros_msg;
-        cmd.motorCmd[i] = rosMsg2Cmd(msg.motorCmd[i]);
+}
 
+unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state)
+{
+    unitree_legged_msgs::HighState ros_msg;
 
-    for(std::size_t i(0); i < 4; ++i)
+    for (int i(0); i < 2; i++)
-        cmd.led[i] = rosMsg2Cmd(msg.led[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 < 40; ++i)
+    for (int i(0); i < 4; i++)
-        cmd.wirelessRemote[i] = msg.wirelessRemote[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]);
+    }
 
-    cmd.reserve = msg.reserve;
+    for (int i(0); i < 3; i++)
-    cmd.crc = msg.crc;
+    {
+        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_

unitree_legged_real/launch/keyboard_control.launch

@@ -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>

unitree_legged_real/package.xml

@@ -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>

unitree_legged_real/src/exe/control_via_keyboard.cpp

@@ -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;
+}

unitree_legged_real/src/exe/example_position.cpp

@@ -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
+    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::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,81 +51,38 @@ 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);
+        if (initiated_flag == true)
-
+        {
-        
             motiontime += 2;
 
-        low_cmd_ros.motorCmd[FR_0].tau = -1.6f;
+            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;
 
-        if (motiontime >= 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;
-        // first, get record initial position
+            low_cmd_ros.motorCmd[FR_2].Kp = 5.0;
-        // if( motiontime >= 100 && motiontime < 500){
+            low_cmd_ros.motorCmd[FR_2].Kd = 1.0;
-            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;
 
-                qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate);
+            low_cmd_ros.motorCmd[FR_0].q = 0.0;
-                qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate);
+            low_cmd_ros.motorCmd[FR_0].dq = 0.0;
-                qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate);
+            low_cmd_ros.motorCmd[FR_0].Kp = 5.0;
-            }
+            low_cmd_ros.motorCmd[FR_0].Kd = 1.0;
-            double sin_joint1, sin_joint2;
+
-        // last, do sine wave
+            low_cmd_ros.motorCmd[FR_1].q = 0.0;
-            float freq_Hz = 1;
+            low_cmd_ros.motorCmd[FR_1].dq = 0.0;
-        // float freq_Hz = 5;
+            low_cmd_ros.motorCmd[FR_1].Kp = 5.0;
-            float freq_rad = freq_Hz * 2 * M_PI;
+            low_cmd_ros.motorCmd[FR_1].Kd = 1.0;
-            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].q = qDes[0];
+        count++;
-            low_cmd_ros.motorCmd[FR_0].dq = 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].tau = -1.6f;
+            initiated_flag = true;
-
-            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_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;
         }
 
-
         pub.publish(low_cmd_ros);
 
         ros::spinOnce();

unitree_legged_real/src/exe/example_walk.cpp

@@ -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;
+        if (motiontime > 0 && motiontime < 1000)
-        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;
+            high_cmd_ros.mode = 1;
+            high_cmd_ros.euler[0] = -0.3;
         }
         if (motiontime > 1000 && motiontime < 2000)
         {
-            high_cmd_ros.mode = 1;    // mode 1: force stand, control robot orientation and height
+            high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[0] = 0.7;
+            high_cmd_ros.euler[0] = 0.3;
         }
         if (motiontime > 2000 && motiontime < 3000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[0] = 0.;
+            high_cmd_ros.euler[1] = -0.2;
         }
         if (motiontime > 3000 && motiontime < 4000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[1] = 0.4;
+            high_cmd_ros.euler[1] = 0.2;
         }
         if (motiontime > 4000 && motiontime < 5000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[1] = 0.;
+            high_cmd_ros.euler[2] = -0.2;
         }
         if (motiontime > 5000 && motiontime < 6000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[2] = 0.5;
+            high_cmd_ros.euler[2] = 0.2;
         }
         if (motiontime > 6000 && motiontime < 7000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.rpy[2] = 0;
+            high_cmd_ros.bodyHeight = -0.2;
+        }
+        if (motiontime > 7000 && motiontime < 8000)
+        {
+            high_cmd_ros.mode = 1;
+            high_cmd_ros.bodyHeight = 0.1;
         }
         if (motiontime > 8000 && motiontime < 9000)
         {
             high_cmd_ros.mode = 1;
-            high_cmd_ros.dBodyHeight = 0.05;
+            high_cmd_ros.bodyHeight = 0.0;
         }
-        if (motiontime>9000 && motiontime <10000)
+        if (motiontime > 9000 && motiontime < 11000)
         {
-            high_cmd_ros.mode = 1;
+            high_cmd_ros.mode = 5;
-            high_cmd_ros.dBodyHeight = -0.15;
-        }
-        if (motiontime>10000 && motiontime <11000)
-        {
-            high_cmd_ros.mode = 1;
-            high_cmd_ros.dBodyHeight = 0.;
         }
         if (motiontime > 11000 && motiontime < 13000)
         {
-            high_cmd_ros.mode = 2;    // mode 2: following tareget velocity in body frame
+            high_cmd_ros.mode = 6;
-            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;
         }
-        if (motiontime>13000 && motiontime <15000)
+        if (motiontime > 13000 && motiontime < 14000)
         {
-            high_cmd_ros.mode = 2;
+            high_cmd_ros.mode = 0;
-            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
         }
-        if (motiontime> 15000 && motiontime <17000)
+        if (motiontime > 14000 && motiontime < 18000)
-        {
-            high_cmd_ros.mode = 0; // normal trot of gaitType 0 will stop automatically
-        }
-        if (motiontime>17000 && motiontime <23000)
-        {
-            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
-        }
-        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)
         {
             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[0] = 0.4f; // -1  ~ +1
-            high_cmd_ros.velocity[1] = 0.;
+            high_cmd_ros.yawSpeed = 2;
-            high_cmd_ros.yawSpeed = 0;
+            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 > 20000 && motiontime < 24000)
-        if (motiontime>42000 && motiontime <44000)
         {
-            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);
 

unitree_legged_real/src/exe/ros_udp.cpp

@@ -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),
+        low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007),
-        high_udp(8081, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState))
+        high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState))
     {
         high_udp.InitCmdData(high_cmd);
         low_udp.InitCmdData(low_cmd);
@@ -81,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;
 
@@ -91,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;
 
@@ -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");

unitree_legged_real/src/exe/state_sub.cpp

@@ -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;
+}

unitree_legged_real/src/exe/twist_sub.cpp

@@ -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;
+}

unitree_legged_sdk

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