add keyboard control function

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.
 

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})
+

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 <unitree_legged_msgs/IMU.h>
 #include "unitree_legged_sdk/unitree_legged_sdk.h"
 #include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
 
 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_
+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/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/ros_udp.cpp

@@ -7,6 +7,7 @@
 #include "convert.h"
 #include <chrono>
 #include <pthread.h>
+#include <geometry_msgs/Twist.h>
 
 using namespace UNITREE_LEGGED_SDK;
 class Custom
@@ -146,4 +147,4 @@ int main(int argc, char **argv)
     }
 
     return 0;
-}
+}

unitree_legged_real/src/exe/twist_sub.cpp

@@ -0,0 +1,110 @@
+#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),
+          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;
+}