From ccb6143b5176f60e69bab45504175a0b83428fd0 Mon Sep 17 00:00:00 2001 From: Bian Zekun <568574579@qq.com> Date: Wed, 26 Aug 2020 15:27:17 +0800 Subject: [PATCH] fit the new unitree_legged_sdk --- unitree_legged_real/src/exe/position_mode.cpp | 13 ++++++++----- unitree_legged_real/src/exe/torque_mode.cpp | 11 ++++++----- unitree_legged_real/src/exe/velocity_mode.cpp | 12 +++++++----- unitree_legged_real/src/exe/walk_mode.cpp | 11 ++++++----- 4 files changed, 27 insertions(+), 20 deletions(-) diff --git a/unitree_legged_real/src/exe/position_mode.cpp b/unitree_legged_real/src/exe/position_mode.cpp index 61fe1a9..0db0b27 100644 --- a/unitree_legged_real/src/exe/position_mode.cpp +++ b/unitree_legged_real/src/exe/position_mode.cpp @@ -35,7 +35,7 @@ double jointLinearInterpolation(double initPos, double targetPos, double rate) } template -int mainHelper(int argc, char *argv[]) +int mainHelper(int argc, char *argv[], TLCM &roslcm) { std::cout << "WARNING: Control level is set to LOW-level." << std::endl << "Make sure the robot is hung up." << std::endl @@ -57,7 +57,7 @@ int mainHelper(int argc, char *argv[]) TState RecvLowLCM = {0}; unitree_legged_msgs::LowCmd SendLowROS; unitree_legged_msgs::LowState RecvLowROS; - TLCM roslcm; + bool initiated_flag = false; // initiate need time int count = 0; @@ -166,7 +166,8 @@ int main(int argc, char *argv[]){ if(firmwork == "3_1"){ aliengo::Control control(aliengo::LOWLEVEL); - mainHelper(argc, argv); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); } else if(firmwork == "3_2"){ std::string robot_name; @@ -177,7 +178,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::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - mainHelper(argc, argv); + // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); + UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/torque_mode.cpp b/unitree_legged_real/src/exe/torque_mode.cpp index ad7d3de..a16de3c 100644 --- a/unitree_legged_real/src/exe/torque_mode.cpp +++ b/unitree_legged_real/src/exe/torque_mode.cpp @@ -26,7 +26,7 @@ void* update_loop(void* param) } template -int mainHelper(int argc, char *argv[]) +int mainHelper(int argc, char *argv[], TLCM &roslcm) { std::cout << "WARNING: Control level is set to LOW-level." << std::endl << "Make sure the robot is hung up." << std::endl @@ -42,7 +42,6 @@ int mainHelper(int argc, char *argv[]) TState RecvLowLCM = {0}; unitree_legged_msgs::LowCmd SendLowROS; unitree_legged_msgs::LowState RecvLowROS; - TLCM roslcm; roslcm.SubscribeState(); @@ -92,7 +91,8 @@ int main(int argc, char *argv[]){ if(firmwork == "3_1"){ aliengo::Control control(aliengo::LOWLEVEL); - mainHelper(argc, argv); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); } else if(firmwork == "3_2"){ std::string robot_name; @@ -103,7 +103,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::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - mainHelper(argc, argv); + UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/velocity_mode.cpp b/unitree_legged_real/src/exe/velocity_mode.cpp index dff7526..4757b8a 100644 --- a/unitree_legged_real/src/exe/velocity_mode.cpp +++ b/unitree_legged_real/src/exe/velocity_mode.cpp @@ -27,7 +27,7 @@ void* update_loop(void* param) } template -int mainHelper(int argc, char *argv[]) +int mainHelper(int argc, char *argv[], TLCM &roslcm) { std::cout << "WARNING: Control level is set to LOW-level." << std::endl << "Make sure the robot is hung up." << std::endl @@ -45,7 +45,6 @@ int mainHelper(int argc, char *argv[]) TState RecvLowLCM = {0}; unitree_legged_msgs::LowCmd SendLowROS; unitree_legged_msgs::LowState RecvLowROS; - TLCM roslcm; roslcm.SubscribeState(); @@ -95,7 +94,8 @@ int main(int argc, char *argv[]){ if(firmwork == "3_1"){ aliengo::Control control(aliengo::LOWLEVEL); - mainHelper(argc, argv); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); } else if(firmwork == "3_2"){ std::string robot_name; @@ -106,7 +106,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::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - mainHelper(argc, argv); + // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); + UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); } } \ No newline at end of file diff --git a/unitree_legged_real/src/exe/walk_mode.cpp b/unitree_legged_real/src/exe/walk_mode.cpp index 2ab3ef2..ebac273 100644 --- a/unitree_legged_real/src/exe/walk_mode.cpp +++ b/unitree_legged_real/src/exe/walk_mode.cpp @@ -27,7 +27,7 @@ void* update_loop(void* param) } template -int mainHelper(int argc, char *argv[]) +int mainHelper(int argc, char *argv[], TLCM &roslcm) { std::cout << "WARNING: Control level is set to HIGH-level." << std::endl << "Make sure the robot is standing on the ground." << std::endl @@ -43,7 +43,6 @@ int mainHelper(int argc, char *argv[]) TState RecvHighLCM = {0}; unitree_legged_msgs::HighCmd SendHighROS; unitree_legged_msgs::HighState RecvHighROS; - TLCM roslcm; roslcm.SubscribeState(); @@ -139,7 +138,8 @@ int main(int argc, char *argv[]){ if(firmwork == "3_1"){ aliengo::Control control(aliengo::HIGHLEVEL); - mainHelper(argc, argv); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); } else if(firmwork == "3_2"){ std::string robot_name; @@ -150,8 +150,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::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - mainHelper(argc, argv); + UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); + mainHelper(argc, argv, roslcm); } } \ No newline at end of file