can compile on Tx2 or NX

README.md

@@ -15,11 +15,14 @@ Real robot control related: `unitree_legged_real`
 # Dependencies
 * [ROS](https://www.ros.org/) melodic or ROS kinetic(has not been tested)
 * [Gazebo8](http://gazebosim.org/)
-* [unitree_legged_sdk](https://github.com/unitreerobotics)
+* [unitree_legged_sdk](https://github.com/unitreerobotics): If your robot is suitable for `unitree_legged_sdk`, then you do not need `aliengo_sdk`.
-* [aliengo_sdk](https://github.com/unitreerobotics)
+* [aliengo_sdk](https://github.com/unitreerobotics): If your robot is suitable for `aliengo_sdk`, then you do not need `unitree_legged_sdk`.
 
 # Configuration
 Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case. 
+If your use `unitree_legged_sdk`, then you need to set `UNITREE_SDK_VERSION=3_2` and the path `UNITREE_LEGGED_SDK_PATH`.
+Otherwise, if you use `aliengo_sdk`, you need to set `UNITREE_SDK_VERSION=3_1` and the path `ALIENGO_SDK_PATH`.
+
 ```
 source /opt/ros/melodic/setup.bash
 source /usr/share/gazebo-8/setup.sh
@@ -27,14 +30,18 @@ source ~/catkin_ws/devel/setup.bash
 export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH}
 export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH}
 export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH}
+# 3_1, 3_2
+export UNITREE_SDK_VERSION=3_2
 export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk
 export ALIENGO_SDK_PATH=~/aliengo_sdk
-#amd64, arm32, arm64
+# amd64, arm32, arm64
 export UNITREE_PLATFORM="amd64"
 ```
 
 # Build
-Please run the following command to install relative packages.
+If you just need to send message from the ROS on robot's NX or TX2, you can just copy `unitree_legged_msgs` and `unitree_legged_real` to `catkin_ws/src` folder and run `catkin_make` to compile.
+
+If you would like to fully compile the `unitree_ros`, please run the following command to install relative packages.
 
 If your ROS is melodic:
 ```
@@ -61,6 +68,7 @@ catkin_make
 
 If you face a dependency problem, you can just run `catkin_make` again.
 
+
 # Detail of Packages
 ## unitree_legged_control:
 It contains the joints controllers for Gazebo simulation, which allows user to control joints with position, velocity and torque.
@@ -148,4 +156,4 @@ torque_lcm
 ```
 The `velocity_lcm` and `torque_lcm` have to run under root account too. Please use the same method as runing `real_launch`.
 
-And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`.
+And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`.

unitree_legged_real/CMakeLists.txt

@@ -4,34 +4,46 @@ project(unitree_legged_real)
 add_compile_options(-std=c++11)
 
 find_package(catkin REQUIRED COMPONENTS
-    controller_manager
     roscpp
-    tf
     geometry_msgs
     unitree_legged_msgs
 )
 
-find_package(Eigen3 REQUIRED)
+catkin_package()
-
-catkin_package(
-)
 
 include_directories(
     include
     ${Boost_INCLUDE_DIR}
     ${catkin_INCLUDE_DIRS}
-    ${EIGEN3_INCLUDE_DIR}
-    $ENV{UNITREE_LEGGED_SDK_PATH}/include
-    $ENV{ALIENGO_SDK_PATH}/include
 )
 
-link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib)
-link_directories($ENV{ALIENGO_SDK_PATH}/lib)
-string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so)
-set(EXTRA_LIBS ${LEGGED_SDK_NAME} libaliengo_comm.so lcm)
-
 set(CMAKE_CXX_FLAGS "-O3")
 
+if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1")
+    include_directories($ENV{ALIENGO_SDK_PATH}/include)
+    link_directories($ENV{ALIENGO_SDK_PATH}/lib)
+    string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so)
+    set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
+
+    add_definitions(-DSDK3_1)
+
+    add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp)
+    target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES})
+    add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+elseif( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_2")
+    include_directories($ENV{UNITREE_LEGGED_SDK_PATH}/include)
+    link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib)
+    string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so)
+    set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm)
+
+    add_definitions(-DSDK3_2)
+
+    add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
+    target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES})
+    add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+endif()
+
 add_executable(position_lcm src/exe/position_mode.cpp)
 target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
 add_dependencies(position_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -46,13 +58,4 @@ add_dependencies(torque_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTE
 
 add_executable(walk_lcm src/exe/walk_mode.cpp)
 target_link_libraries(walk_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES})
-add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-
-add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp)
-target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES})
-add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp)
-target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES})
-add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

unitree_legged_real/include/convert.h

@@ -6,7 +6,6 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #ifndef _CONVERT_H_
 #define _CONVERT_H_
 
-#include "unitree_legged_sdk/unitree_legged_sdk.h"
 #include <unitree_legged_msgs/LowCmd.h>
 #include <unitree_legged_msgs/LowState.h>
 #include <unitree_legged_msgs/HighCmd.h>
@@ -14,12 +13,158 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #include <unitree_legged_msgs/MotorCmd.h>
 #include <unitree_legged_msgs/MotorState.h>
 #include <unitree_legged_msgs/IMU.h>
+
+#ifdef SDK3_1
 #include "aliengo_sdk/aliengo_sdk.hpp"
 
-enum firmworkVersion{
+unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){
-    V3_1,
+    unitree_legged_msgs::IMU ros;
-    V3_2
+    ros.quaternion[0] = lcm.quaternion[0];
-};
+    ros.quaternion[1] = lcm.quaternion[1];
+    ros.quaternion[2] = lcm.quaternion[2];
+    ros.quaternion[3] = lcm.quaternion[3];
+    ros.gyroscope[0] = lcm.gyroscope[0];
+    ros.gyroscope[1] = lcm.gyroscope[1];
+    ros.gyroscope[2] = lcm.gyroscope[2];
+    ros.accelerometer[0] = lcm.acceleration[0];
+    ros.accelerometer[1] = lcm.acceleration[1];
+    ros.accelerometer[2] = lcm.acceleration[2];
+    // ros.rpy[0] = lcm.rpy[0];
+    // ros.rpy[1] = lcm.rpy[1];
+    // ros.rpy[2] = lcm.rpy[2];
+    ros.temperature = lcm.temp;
+    return ros;
+}
+
+unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){
+    unitree_legged_msgs::MotorState ros;
+    ros.mode = lcm.mode;
+    ros.q = lcm.position;
+    ros.dq = lcm.velocity;
+    ros.ddq = 0;
+    ros.tauEst = lcm.torque;
+    ros.q_raw = 0;
+    ros.dq_raw = 0;
+    ros.ddq_raw = 0;
+    ros.temperature = lcm.temperature;
+    return ros;
+}
+
+aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){
+    aliengo::MotorCmd lcm;
+    lcm.mode = ros.mode;
+    lcm.position = ros.q;
+    lcm.velocity = ros.dq;
+    lcm.positionStiffness = ros.Kp;
+    lcm.velocityStiffness = ros.Kd;
+    lcm.torque = ros.tau;
+    return lcm;
+}
+
+unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){
+    unitree_legged_msgs::LowState ros;
+    ros.levelFlag = lcm.levelFlag;
+    ros.commVersion = 0;
+    ros.robotID = 0;
+    ros.SN = 0;
+    ros.bandWidth = 0;
+    ros.imu = ToRos(lcm.imu);
+    for(int i = 0; i < 20; i++){
+        ros.motorState[i] = ToRos(lcm.motorState[i]);
+    }
+    for(int i = 0; i < 4; i++){
+        ros.footForce[i] = lcm.footForce[i];
+        ros.footForceEst[i] = 0;
+    }
+    ros.tick = lcm.tick;
+    for(int i = 0; i < 40; i++){
+        ros.wirelessRemote[i] = lcm.wirelessRemote[i];
+    }
+    ros.crc = lcm.crc;
+
+    return ros;
+}
+
+aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){
+    aliengo::LowCmd lcm;
+    lcm.levelFlag = ros.levelFlag;
+    for(int i = 0; i < 20; i++){
+        lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
+    }
+    for(int i = 0; i < 4; i++){
+        lcm.led[i].r = ros.led[i].r;
+        lcm.led[i].g = ros.led[i].g;
+        lcm.led[i].b = ros.led[i].b;
+    }
+    for(int i = 0; i < 40; i++){
+        lcm.wirelessRemote[i] = ros.wirelessRemote[i];
+    }
+    lcm.crc = ros.crc;
+    return lcm;
+}
+
+unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){
+    unitree_legged_msgs::HighState ros;
+    ros.levelFlag = lcm.levelFlag;
+    ros.commVersion = 0;
+    ros.robotID = 0;
+    ros.SN = 0;
+    ros.bandWidth = 0;
+    ros.mode = lcm.mode;
+    ros.imu = ToRos(lcm.imu);
+    ros.forwardSpeed = lcm.forwardSpeed;
+    ros.sideSpeed = lcm.sideSpeed;
+    ros.rotateSpeed = lcm.rotateSpeed;
+    ros.bodyHeight = lcm.bodyHeight;
+    ros.updownSpeed = lcm.updownSpeed;
+    ros.forwardPosition = lcm.forwardPosition.x;
+    ros.sidePosition = lcm.sidePosition.y;
+    for(int i = 0; i < 4; i++){
+        ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
+        ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
+        ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
+        ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
+        ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
+        ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
+        ros.footForce[i] = lcm.footForce[i];
+        ros.footForceEst[i] = 0;
+    }
+    ros.tick = lcm.tick;
+    for(int i = 0; i<40; i++){
+        ros.wirelessRemote[i] = lcm.wirelessRemote[i];
+    }
+    ros.reserve = 0;
+    ros.crc = lcm.crc;
+    return ros;
+}
+
+aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){
+    aliengo::HighCmd lcm;
+    lcm.levelFlag = ros.levelFlag;
+    lcm.mode = ros.mode;
+    lcm.forwardSpeed = ros.forwardSpeed;
+    lcm.sideSpeed = ros.sideSpeed;
+    lcm.rotateSpeed = ros.rotateSpeed;
+    lcm.bodyHeight = ros.bodyHeight;
+    lcm.footRaiseHeight = ros.footRaiseHeight;
+    lcm.yaw = ros.yaw;
+    lcm.pitch = ros.pitch;
+    lcm.roll = ros.roll;
+    for(int i = 0; i < 4; i++){
+        lcm.led[i].r = ros.led[i].r;
+        lcm.led[i].g = ros.led[i].g;
+        lcm.led[i].b = ros.led[i].b;
+    }
+    for(int i = 0; i < 40; i++){
+        lcm.wirelessRemote[i] = ros.wirelessRemote[i];
+    }
+    lcm.crc = ros.crc;
+    return lcm;
+}
+#endif
+
+#ifdef SDK3_2
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
 
 unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
 {
@@ -41,25 +186,6 @@ unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm)
     return ros;
 }
 
-unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){
-    unitree_legged_msgs::IMU ros;
-    ros.quaternion[0] = lcm.quaternion[0];
-    ros.quaternion[1] = lcm.quaternion[1];
-    ros.quaternion[2] = lcm.quaternion[2];
-    ros.quaternion[3] = lcm.quaternion[3];
-    ros.gyroscope[0] = lcm.gyroscope[0];
-    ros.gyroscope[1] = lcm.gyroscope[1];
-    ros.gyroscope[2] = lcm.gyroscope[2];
-    ros.accelerometer[0] = lcm.acceleration[0];
-    ros.accelerometer[1] = lcm.acceleration[1];
-    ros.accelerometer[2] = lcm.acceleration[2];
-    // ros.rpy[0] = lcm.rpy[0];
-    // ros.rpy[1] = lcm.rpy[1];
-    // ros.rpy[2] = lcm.rpy[2];
-    ros.temperature = lcm.temp;
-    return ros;
-}
-
 unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
 {
     unitree_legged_msgs::MotorState ros;
@@ -77,23 +203,6 @@ unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm)
     return ros;
 }
 
-unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){
-    unitree_legged_msgs::MotorState ros;
-    ros.mode = lcm.mode;
-    ros.q = lcm.position;
-    ros.dq = lcm.velocity;
-    ros.ddq = 0;
-    ros.tauEst = lcm.torque;
-    ros.q_raw = 0;
-    ros.dq_raw = 0;
-    ros.ddq_raw = 0;
-    ros.temperature = lcm.temperature;
-
-// printf("dq: %f\n", lcm.velocity);
-
-    return ros;
-}
-
 UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType)
 {
     UNITREE_LEGGED_SDK::MotorCmd lcm;
@@ -109,17 +218,6 @@ UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_L
     return lcm;
 }
 
-aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){
-    aliengo::MotorCmd lcm;
-    lcm.mode = ros.mode;
-    lcm.position = ros.q;
-    lcm.velocity = ros.dq;
-    lcm.positionStiffness = ros.Kp;
-    lcm.velocityStiffness = ros.Kd;
-    lcm.torque = ros.tau;
-    return lcm;
-}
-
 unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
 {
     unitree_legged_msgs::LowState ros;
@@ -145,30 +243,6 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm)
     return ros;
 }
 
-unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){
-    unitree_legged_msgs::LowState ros;
-    ros.levelFlag = lcm.levelFlag;
-    ros.commVersion = 0;
-    ros.robotID = 0;
-    ros.SN = 0;
-    ros.bandWidth = 0;
-    ros.imu = ToRos(lcm.imu);
-    for(int i = 0; i < 20; i++){
-        ros.motorState[i] = ToRos(lcm.motorState[i]);
-    }
-    for(int i = 0; i < 4; i++){
-        ros.footForce[i] = lcm.footForce[i];
-        ros.footForceEst[i] = 0;
-    }
-    ros.tick = lcm.tick;
-    for(int i = 0; i < 40; i++){
-        ros.wirelessRemote[i] = lcm.wirelessRemote[i];
-    }
-    ros.crc = lcm.crc;
-
-    return ros;
-}
-
 UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType)
 {
     UNITREE_LEGGED_SDK::LowCmd lcm;
@@ -193,24 +267,6 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGE
     return lcm;
 }
 
-aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){
-    aliengo::LowCmd lcm;
-    lcm.levelFlag = ros.levelFlag;
-    for(int i = 0; i < 20; i++){
-        lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]);
-    }
-    for(int i = 0; i < 4; i++){
-        lcm.led[i].r = ros.led[i].r;
-        lcm.led[i].g = ros.led[i].g;
-        lcm.led[i].b = ros.led[i].b;
-    }
-    for(int i = 0; i < 40; i++){
-        lcm.wirelessRemote[i] = ros.wirelessRemote[i];
-    }
-    lcm.crc = ros.crc;
-    return lcm;
-}
-
 unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
 {
     unitree_legged_msgs::HighState ros;
@@ -247,41 +303,6 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm)
     return ros;
 }
 
-unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){
-    unitree_legged_msgs::HighState ros;
-    ros.levelFlag = lcm.levelFlag;
-    ros.commVersion = 0;
-    ros.robotID = 0;
-    ros.SN = 0;
-    ros.bandWidth = 0;
-    ros.mode = lcm.mode;
-    ros.imu = ToRos(lcm.imu);
-    ros.forwardSpeed = lcm.forwardSpeed;
-    ros.sideSpeed = lcm.sideSpeed;
-    ros.rotateSpeed = lcm.rotateSpeed;
-    ros.bodyHeight = lcm.bodyHeight;
-    ros.updownSpeed = lcm.updownSpeed;
-    ros.forwardPosition = lcm.forwardPosition.x;
-    ros.sidePosition = lcm.sidePosition.y;
-    for(int i = 0; i < 4; i++){
-        ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x;
-        ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y;
-        ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z;
-        ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x;
-        ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y;
-        ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z;
-        ros.footForce[i] = lcm.footForce[i];
-        ros.footForceEst[i] = 0;
-    }
-    ros.tick = lcm.tick;
-    for(int i = 0; i<40; i++){
-        ros.wirelessRemote[i] = lcm.wirelessRemote[i];
-    }
-    ros.reserve = 0;
-    ros.crc = lcm.crc;
-    return ros;
-}
-
 UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType)
 {
     UNITREE_LEGGED_SDK::HighCmd lcm;
@@ -312,30 +333,6 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG
     lcm.crc = ros.crc;
     return lcm;
 }
+#endif
 
-aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){
+#endif  // _CONVERT_H_
-    aliengo::HighCmd lcm;
-    lcm.levelFlag = ros.levelFlag;
-    lcm.mode = ros.mode;
-    lcm.forwardSpeed = ros.forwardSpeed;
-    lcm.sideSpeed = ros.sideSpeed;
-    lcm.rotateSpeed = ros.rotateSpeed;
-    lcm.bodyHeight = ros.bodyHeight;
-    lcm.footRaiseHeight = ros.footRaiseHeight;
-    lcm.yaw = ros.yaw;
-    lcm.pitch = ros.pitch;
-    lcm.roll = ros.roll;
-    for(int i = 0; i < 4; i++){
-        lcm.led[i].r = ros.led[i].r;
-        lcm.led[i].g = ros.led[i].g;
-        lcm.led[i].b = ros.led[i].b;
-    }
-    for(int i = 0; i < 40; i++){
-        lcm.wirelessRemote[i] = ros.wirelessRemote[i];
-    }
-    lcm.crc = ros.crc;
-    return lcm;
-}
-
-
-#endif

unitree_legged_real/src/exe/position_mode.cpp

@@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #include <boost/thread/mutex.hpp>
 #include <unitree_legged_msgs/LowCmd.h>
 #include <unitree_legged_msgs/LowState.h>
-#include "unitree_legged_sdk/unitree_legged_sdk.h"
-#include "aliengo_sdk/aliengo_sdk.hpp"
 #include "convert.h"
 
+#ifdef SDK3_1
+using namespace aliengo;
+#endif
+#ifdef SDK3_2
 using namespace UNITREE_LEGGED_SDK;
+#endif
 
 template<typename TLCM>
 void* update_loop(void* param)
@@ -164,12 +167,13 @@ int main(int argc, char *argv[]){
     std::string firmwork;
     ros::param::get("/firmwork", firmwork);
 
-    if(firmwork == "3_1"){
+    #ifdef SDK3_1
         aliengo::Control control(aliengo::LOWLEVEL);
         aliengo::LCM roslcm;
         mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
-    }
+    #endif
-    else if(firmwork == "3_2"){
+
+    #ifdef SDK3_2
         std::string robot_name;
         UNITREE_LEGGED_SDK::LeggedType rname;
         ros::param::get("/robot_name", robot_name);
@@ -179,8 +183,8 @@ int main(int argc, char *argv[]){
             rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
             
         // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
-        UNITREE_LEGGED_SDK::InitEnvironment();
+        // UNITREE_LEGGED_SDK::InitEnvironment();
         UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
         mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
-    }
+    #endif
 }

unitree_legged_real/src/exe/torque_mode.cpp

@@ -9,11 +9,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #include <boost/thread/mutex.hpp>
 #include <unitree_legged_msgs/LowCmd.h>
 #include <unitree_legged_msgs/LowState.h>
-#include "unitree_legged_sdk/unitree_legged_sdk.h"
-#include "aliengo_sdk/aliengo_sdk.hpp"
 #include "convert.h"
 
+#ifdef SDK3_1
+using namespace aliengo;
+#endif
+#ifdef SDK3_2
 using namespace UNITREE_LEGGED_SDK;
+#endif
 
 template<typename TLCM>
 void* update_loop(void* param)
@@ -89,12 +92,13 @@ int main(int argc, char *argv[]){
     std::string firmwork;
     ros::param::get("/firmwork", firmwork);
 
-    if(firmwork == "3_1"){
+    #ifdef SDK3_1
         aliengo::Control control(aliengo::LOWLEVEL);
         aliengo::LCM roslcm;
         mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
-    }
+    #endif
-    else if(firmwork == "3_2"){
+
+    #ifdef SDK3_2
         std::string robot_name;
         UNITREE_LEGGED_SDK::LeggedType rname;
         ros::param::get("/robot_name", robot_name);
@@ -103,8 +107,8 @@ int main(int argc, char *argv[]){
         else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
             rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
             
-        UNITREE_LEGGED_SDK::InitEnvironment();
+        // UNITREE_LEGGED_SDK::InitEnvironment();
         UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
         mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
-    }
+    #endif
 }

unitree_legged_real/src/exe/velocity_mode.cpp

@@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #include <boost/thread/mutex.hpp>
 #include <unitree_legged_msgs/LowCmd.h>
 #include <unitree_legged_msgs/LowState.h>
-#include "unitree_legged_sdk/unitree_legged_sdk.h"
-#include "aliengo_sdk/aliengo_sdk.hpp"
 #include "convert.h"
 
+#ifdef SDK3_1
+using namespace aliengo;
+#endif
+#ifdef SDK3_2
 using namespace UNITREE_LEGGED_SDK;
+#endif
 
 template<typename TLCM>
 void* update_loop(void* param)
@@ -92,12 +95,13 @@ int main(int argc, char *argv[]){
     std::string firmwork;
     ros::param::get("/firmwork", firmwork);
 
-    if(firmwork == "3_1"){
+    #ifdef SDK3_1
         aliengo::Control control(aliengo::LOWLEVEL);
         aliengo::LCM roslcm;
         mainHelper<aliengo::LowCmd, aliengo::LowState, aliengo::LCM>(argc, argv, roslcm);
-    }
+    #endif
-    else if(firmwork == "3_2"){
+
+    #ifdef SDK3_2
         std::string robot_name;
         UNITREE_LEGGED_SDK::LeggedType rname;
         ros::param::get("/robot_name", robot_name);
@@ -107,8 +111,8 @@ int main(int argc, char *argv[]){
             rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
             
         // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL);
-        UNITREE_LEGGED_SDK::InitEnvironment();
+        // UNITREE_LEGGED_SDK::InitEnvironment();
         UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL);
         mainHelper<UNITREE_LEGGED_SDK::LowCmd, UNITREE_LEGGED_SDK::LowState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
-    }
+    #endif
 }

unitree_legged_real/src/exe/walk_mode.cpp

@@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #include <boost/thread/mutex.hpp>
 #include <unitree_legged_msgs/HighCmd.h>
 #include <unitree_legged_msgs/HighState.h>
-#include "unitree_legged_sdk/unitree_legged_sdk.h"
-#include "aliengo_sdk/aliengo_sdk.hpp"
 #include "convert.h"
 
-// using namespace UNITREE_LEGGED_SDK;
+#ifdef SDK3_1
+using namespace aliengo;
+#endif
+#ifdef SDK3_2
+using namespace UNITREE_LEGGED_SDK;
+#endif
 
 template<typename TLCM>
 void* update_loop(void* param)
@@ -136,12 +139,13 @@ int main(int argc, char *argv[]){
     std::string firmwork;
     ros::param::get("/firmwork", firmwork);
 
-    if(firmwork == "3_1"){
+    #ifdef SDK3_1
         aliengo::Control control(aliengo::HIGHLEVEL);
         aliengo::LCM roslcm;
         mainHelper<aliengo::HighCmd, aliengo::HighState, aliengo::LCM>(argc, argv, roslcm);
-    }
+    #endif
-    else if(firmwork == "3_2"){
+
+    #ifdef SDK3_2
         std::string robot_name;
         UNITREE_LEGGED_SDK::LeggedType rname;
         ros::param::get("/robot_name", robot_name);
@@ -150,9 +154,9 @@ int main(int argc, char *argv[]){
         else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0)
             rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo;
 
-        UNITREE_LEGGED_SDK::InitEnvironment();
+        // UNITREE_LEGGED_SDK::InitEnvironment();
         UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL);
         mainHelper<UNITREE_LEGGED_SDK::HighCmd, UNITREE_LEGGED_SDK::HighState, UNITREE_LEGGED_SDK::LCM>(argc, argv, roslcm);
-    }
+    #endif
     
 }