From 05e91528c135e580c880b3375c544e818d12e68a Mon Sep 17 00:00:00 2001 From: Your Name Date: Fri, 27 May 2022 11:24:44 +0800 Subject: [PATCH] add keyboard control function --- README.md | 5 + unitree_legged_real/CMakeLists.txt | 8 ++ unitree_legged_real/include/convert.h | 33 +++++- .../launch/keyboard_control.launch | 8 ++ .../src/exe/control_via_keyboard.cpp | 111 ++++++++++++++++++ unitree_legged_real/src/exe/ros_udp.cpp | 3 +- unitree_legged_real/src/exe/twist_sub.cpp | 110 +++++++++++++++++ 7 files changed, 276 insertions(+), 2 deletions(-) create mode 100644 unitree_legged_real/launch/keyboard_control.launch create mode 100644 unitree_legged_real/src/exe/control_via_keyboard.cpp create mode 100644 unitree_legged_real/src/exe/twist_sub.cpp diff --git a/README.md b/README.md index 320a890..e5157b6 100644 --- a/README.md +++ b/README.md @@ -79,6 +79,11 @@ You can also run the node state_sub to subscribe the feedback information from G rosrun unitree_legged_real state_sub ``` +You can also run the launch file that enables you control robot via keyboard like you can do in turtlesim package +``` +roslaunch unitree_legged_real keyboard_control.launch +``` + And before you do the low-level control, please press L2+A to sit the robot down and then press L1+L2+start to make the robot into mode in which you can do joint-level control, finally make sure you hang the robot up before you run low-level control. diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt index 84c3d19..d7c24e7 100755 --- a/unitree_legged_real/CMakeLists.txt +++ b/unitree_legged_real/CMakeLists.txt @@ -55,3 +55,11 @@ 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 index 9712648..f71a838 100755 --- a/unitree_legged_real/include/convert.h +++ b/unitree_legged_real/include/convert.h @@ -17,6 +17,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include "unitree_legged_sdk/unitree_legged_sdk.h" #include +#include UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) { @@ -300,4 +301,34 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state return ros_msg; } -#endif // _CONVERT_H_ \ No newline at end of file +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 new file mode 100644 index 0000000..d7b877a --- /dev/null +++ b/unitree_legged_real/launch/keyboard_control.launch @@ -0,0 +1,8 @@ + + + + + + + + \ 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 new file mode 100644 index 0000000..53fda22 --- /dev/null +++ b/unitree_legged_real/src/exe/control_via_keyboard.cpp @@ -0,0 +1,111 @@ +#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/ros_udp.cpp b/unitree_legged_real/src/exe/ros_udp.cpp index c16961b..1a3daa9 100644 --- a/unitree_legged_real/src/exe/ros_udp.cpp +++ b/unitree_legged_real/src/exe/ros_udp.cpp @@ -7,6 +7,7 @@ #include "convert.h" #include #include +#include using namespace UNITREE_LEGGED_SDK; class Custom @@ -146,4 +147,4 @@ int main(int argc, char **argv) } 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 new file mode 100644 index 0000000..10c2689 --- /dev/null +++ b/unitree_legged_real/src/exe/twist_sub.cpp @@ -0,0 +1,110 @@ +#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), + 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; +} \ No newline at end of file