diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt deleted file mode 100755 index d0f1fff..0000000 --- a/unitree_legged_real/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(unitree_legged_real) - -add_compile_options(-std=c++11) - -find_package(catkin REQUIRED COMPONENTS - roscpp - geometry_msgs - unitree_legged_msgs -) - -catkin_package() - -message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}") -if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*") - set(ARCH amd64) -else() - set(ARCH arm64) -endif() - -link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH}) - -set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) - -set(CMAKE_CXX_FLAGS "-O3 -fPIC") - -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${CMAKE_SOURCE_DIR}/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}) - -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}) - diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h deleted file mode 100755 index f71a838..0000000 --- a/unitree_legged_real/include/convert.h +++ /dev/null @@ -1,334 +0,0 @@ -/************************************************************************ -Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. -Use of this source code is governed by the MPL-2.0 license, see LICENSE. -************************************************************************/ - -#ifndef _CONVERT_H_ -#define _CONVERT_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include -#include - -UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) -{ - UNITREE_LEGGED_SDK::BmsCmd cmd; - - cmd.off = msg.off; - - for (int 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_SDK::HighCmd cmd; - - for (int i(0); i < 2; i++) - { - cmd.head[i] = msg->head[i]; - cmd.SN[i] = msg->SN[i]; - cmd.version[i] = msg->version[i]; - cmd.position[i] = msg->position[i]; - cmd.velocity[i] = msg->velocity[i]; - } - - for (int i(0); i < 3; i++) - { - cmd.euler[i] = msg->euler[i]; - } - - for (int i(0); i < 4; i++) - { - cmd.led[i].r = msg->led[i].r; - cmd.led[i].g = msg->led[i].g; - cmd.led[i].b = msg->led[i].b; - } - - for (int i(0); i < 40; i++) - { - cmd.wirelessRemote[i] = msg->wirelessRemote[i]; - } - - cmd.levelFlag = msg->levelFlag; - cmd.frameReserve = msg->frameReserve; - cmd.bandWidth = msg->bandWidth; - cmd.mode = msg->mode; - cmd.gaitType = msg->gaitType; - cmd.speedLevel = msg->speedLevel; - cmd.footRaiseHeight = msg->footRaiseHeight; - cmd.bodyHeight = msg->bodyHeight; - cmd.yawSpeed = msg->yawSpeed; - cmd.reserve = msg->reserve; - cmd.crc = msg->crc; - - cmd.bms = rosMsg2Cmd(msg->bms); - - return cmd; -} - -UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg) -{ - UNITREE_LEGGED_SDK::MotorCmd cmd; - - cmd.mode = msg.mode; - cmd.q = msg.q; - cmd.dq = msg.dq; - cmd.tau = msg.tau; - cmd.Kp = msg.Kp; - cmd.Kd = msg.Kd; - - 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) -{ - unitree_legged_msgs::MotorState ros_msg; - - ros_msg.mode = state.mode; - ros_msg.q = state.q; - ros_msg.dq = state.dq; - ros_msg.ddq = state.ddq; - ros_msg.tauEst = state.tauEst; - ros_msg.q_raw = state.q_raw; - ros_msg.dq_raw = state.dq_raw; - ros_msg.ddq_raw = state.ddq_raw; - ros_msg.temperature = state.temperature; - - ros_msg.reserve[0] = state.reserve[0]; - ros_msg.reserve[1] = state.reserve[1]; - - return ros_msg; -} - -unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state) -{ - unitree_legged_msgs::IMU ros_msg; - - for (int i(0); i < 4; i++) - { - ros_msg.quaternion[i] = state.quaternion[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]; - } - - ros_msg.temperature = state.temperature; - - return ros_msg; -} - -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 (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; - - return ros_msg; -} - -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; -} - -unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state) -{ - unitree_legged_msgs::HighState 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]; - ros_msg.rangeObstacle[i] = state.rangeObstacle[i]; - ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]); - ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]); - } - - for (int i(0); i < 3; i++) - { - ros_msg.position[i] = state.position[i]; - ros_msg.velocity[i] = state.velocity[i]; - } - - for (int i(0); i < 40; i++) - { - ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; - } - - for (int i(0); i < 20; i++) - { - ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); - } - - ros_msg.imu = state2rosMsg(state.imu); - - ros_msg.bms = state2rosMsg(state.bms); - - ros_msg.levelFlag = state.levelFlag; - ros_msg.frameReserve = state.frameReserve; - ros_msg.bandWidth = state.bandWidth; - ros_msg.mode = state.mode; - ros_msg.progress = state.progress; - ros_msg.gaitType = state.gaitType; - ros_msg.footRaiseHeight = state.footRaiseHeight; - ros_msg.bodyHeight = state.bodyHeight; - ros_msg.yawSpeed = state.yawSpeed; - ros_msg.reserve = state.reserve; - ros_msg.crc = state.crc; - - return ros_msg; -} - -UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg) -{ - UNITREE_LEGGED_SDK::HighCmd cmd; - - cmd.head[0] = 0xFE; - cmd.head[1] = 0xEF; - cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL; - cmd.mode = 0; - cmd.gaitType = 0; - cmd.speedLevel = 0; - cmd.footRaiseHeight = 0; - cmd.bodyHeight = 0; - cmd.euler[0] = 0; - cmd.euler[1] = 0; - cmd.euler[2] = 0; - cmd.velocity[0] = 0.0f; - cmd.velocity[1] = 0.0f; - cmd.yawSpeed = 0.0f; - cmd.reserve = 0; - - cmd.velocity[0] = msg->linear.x; - cmd.velocity[1] = msg->linear.y; - cmd.yawSpeed = msg->angular.z; - - cmd.mode = 2; - cmd.gaitType = 1; - - return cmd; -} - -#endif // _CONVERT_H_ diff --git a/unitree_legged_real/launch/keyboard_control.launch b/unitree_legged_real/launch/keyboard_control.launch deleted file mode 100644 index d7b877a..0000000 --- a/unitree_legged_real/launch/keyboard_control.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/unitree_legged_real/launch/real.launch b/unitree_legged_real/launch/real.launch deleted file mode 100644 index d0e4ada..0000000 --- a/unitree_legged_real/launch/real.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/unitree_legged_real/package.xml b/unitree_legged_real/package.xml deleted file mode 100644 index 68c09e5..0000000 --- a/unitree_legged_real/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - unitree_legged_real - 0.0.0 - The unitree_legged_real package - - unitree - TODO - - catkin - roscpp - std_msgs - roscpp - std_msgs - roscpp - std_msgs - unitree_legged_msgs - eigen - - \ No newline at end of file diff --git a/unitree_legged_real/src/exe/control_via_keyboard.cpp b/unitree_legged_real/src/exe/control_via_keyboard.cpp deleted file mode 100644 index 53fda22..0000000 --- a/unitree_legged_real/src/exe/control_via_keyboard.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include "ros/ros.h" -#include -#include - -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("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; -} diff --git a/unitree_legged_real/src/exe/example_position.cpp b/unitree_legged_real/src/exe/example_position.cpp deleted file mode 100644 index 7926577..0000000 --- a/unitree_legged_real/src/exe/example_position.cpp +++ /dev/null @@ -1,93 +0,0 @@ -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "convert.h" - -using namespace UNITREE_LEGGED_SDK; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "example_postition_without_lcm"); - - std::cout << "Communication level is set to LOW-level." << std::endl - << "WARNING: Make sure the robot is hung up." << std::endl - << "Press Enter to continue..." << std::endl; - std::cin.ignore(); - - 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}; - - unitree_legged_msgs::LowCmd low_cmd_ros; - - bool initiated_flag = false; // initiate need time - int count = 0; - - ros::Publisher pub = nh.advertise("low_cmd", 1); - - low_cmd_ros.head[0] = 0xFE; - low_cmd_ros.head[1] = 0xEF; - low_cmd_ros.levelFlag = LOWLEVEL; - - 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; // 禁止位置环 - low_cmd_ros.motorCmd[i].Kp = 0; - low_cmd_ros.motorCmd[i].dq = VelStopF; // 禁止速度环 - low_cmd_ros.motorCmd[i].Kd = 0; - low_cmd_ros.motorCmd[i].tau = 0; - } - - while (ros::ok()) - { - - if (initiated_flag == true) - { - motiontime += 2; - - low_cmd_ros.motorCmd[FR_0].tau = -0.65f; - low_cmd_ros.motorCmd[FL_0].tau = +0.65f; - low_cmd_ros.motorCmd[RR_0].tau = -0.65f; - low_cmd_ros.motorCmd[RL_0].tau = +0.65f; - - low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3); - low_cmd_ros.motorCmd[FR_2].dq = 0.0; - low_cmd_ros.motorCmd[FR_2].Kp = 5.0; - low_cmd_ros.motorCmd[FR_2].Kd = 1.0; - - low_cmd_ros.motorCmd[FR_0].q = 0.0; - low_cmd_ros.motorCmd[FR_0].dq = 0.0; - low_cmd_ros.motorCmd[FR_0].Kp = 5.0; - low_cmd_ros.motorCmd[FR_0].Kd = 1.0; - - low_cmd_ros.motorCmd[FR_1].q = 0.0; - low_cmd_ros.motorCmd[FR_1].dq = 0.0; - low_cmd_ros.motorCmd[FR_1].Kp = 5.0; - low_cmd_ros.motorCmd[FR_1].Kd = 1.0; - } - - count++; - if (count > 10) - { - count = 10; - initiated_flag = true; - } - - pub.publish(low_cmd_ros); - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/example_walk.cpp b/unitree_legged_real/src/exe/example_walk.cpp deleted file mode 100644 index ce46ddb..0000000 --- a/unitree_legged_real/src/exe/example_walk.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "convert.h" - -using namespace UNITREE_LEGGED_SDK; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "example_walk_without_lcm"); - - std::cout << "WARNING: Control level is set to HIGH-level." << std::endl - << "Make sure the robot is standing on the ground." << std::endl - << "Press Enter to continue..." << std::endl; - std::cin.ignore(); - - ros::NodeHandle nh; - - ros::Rate loop_rate(500); - - long motiontime = 0; - - unitree_legged_msgs::HighCmd high_cmd_ros; - - ros::Publisher pub = nh.advertise("high_cmd", 1000); - - while (ros::ok()) - { - - 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.yawSpeed = 0.0f; - high_cmd_ros.reserve = 0; - - if (motiontime > 0 && motiontime < 1000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[0] = -0.3; - } - if (motiontime > 1000 && motiontime < 2000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[0] = 0.3; - } - if (motiontime > 2000 && motiontime < 3000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[1] = -0.2; - } - if (motiontime > 3000 && motiontime < 4000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[1] = 0.2; - } - if (motiontime > 4000 && motiontime < 5000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[2] = -0.2; - } - if (motiontime > 5000 && motiontime < 6000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.euler[2] = 0.2; - } - if (motiontime > 6000 && motiontime < 7000) - { - high_cmd_ros.mode = 1; - 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.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) - { - 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"); - } - if (motiontime > 18000 && motiontime < 20000) - { - high_cmd_ros.mode = 0; - high_cmd_ros.velocity[0] = 0; - } - if (motiontime > 20000 && motiontime < 24000) - { - 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 > 24000) - { - high_cmd_ros.mode = 1; - } - - pub.publish(high_cmd_ros); - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/ros_udp.cpp b/unitree_legged_real/src/exe/ros_udp.cpp deleted file mode 100644 index f467aa1..0000000 --- a/unitree_legged_real/src/exe/ros_udp.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "convert.h" -#include -#include -#include - -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_high; -ros::Subscriber sub_low; - -ros::Publisher pub_high; -ros::Publisher pub_low; - -long high_count = 0; -long low_count = 0; - -void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg) -{ - printf("highCmdCallback is running !\t%ld\n", ::high_count); - - custom.high_cmd = rosMsg2Cmd(msg); - - unitree_legged_msgs::HighState high_state_ros; - - high_state_ros = state2rosMsg(custom.high_state); - - pub_high.publish(high_state_ros); - - printf("highCmdCallback ending !\t%ld\n\n", ::high_count++); -} - -void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg) -{ - - printf("lowCmdCallback is running !\t%ld\n", low_count); - - custom.low_cmd = rosMsg2Cmd(msg); - - unitree_legged_msgs::LowState low_state_ros; - - low_state_ros = state2rosMsg(custom.low_state); - - pub_low.publish(low_state_ros); - - printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "ros_udp"); - - ros::NodeHandle nh; - - if (strcasecmp(argv[1], "LOWLEVEL") == 0) - { - sub_low = nh.subscribe("low_cmd", 1, lowCmdCallback); - pub_low = nh.advertise("low_state", 1); - - LoopFunc loop_udpSend("low_udp_send", 0.002, 3, boost::bind(&Custom::lowUdpSend, &custom)); - LoopFunc loop_udpRecv("low_udp_recv", 0.002, 3, boost::bind(&Custom::lowUdpRecv, &custom)); - - loop_udpSend.start(); - loop_udpRecv.start(); - - ros::spin(); - - // printf("low level runing!\n"); - } - else if (strcasecmp(argv[1], "HIGHLEVEL") == 0) - { - sub_high = nh.subscribe("high_cmd", 1, highCmdCallback); - pub_high = nh.advertise("high_state", 1); - - 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(); - - // printf("high level runing!\n"); - } - else - { - std::cout << "Control level name error! Can only be highlevel or lowlevel(not case sensitive)" << std::endl; - exit(-1); - } - - return 0; -} diff --git a/unitree_legged_real/src/exe/state_sub.cpp b/unitree_legged_real/src/exe/state_sub.cpp deleted file mode 100644 index b52ed43..0000000 --- a/unitree_legged_real/src/exe/state_sub.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include - -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; -} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/twist_sub.cpp b/unitree_legged_real/src/exe/twist_sub.cpp deleted file mode 100644 index 7664a09..0000000 --- a/unitree_legged_real/src/exe/twist_sub.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "convert.h" -#include -#include -#include - -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("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; -}