[#feature] simple text processing

go2_robot_sdk/go2_robot_sdk/go2_driver_node.py

@@ -43,7 +43,7 @@
 from go2_interfaces.msg import Go2State, IMU
 from sensor_msgs.msg import PointCloud2, PointField, JointState, Joy
 from sensor_msgs_py import point_cloud2
-from std_msgs.msg import Header
+from std_msgs.msg import Header, String
 
 
 logging.basicConfig(level=logging.WARN)
@@ -76,6 +76,7 @@ def __init__(self):
         self.robot_low_cmd = None
         self.robot_sport_state = None
         self.robot_lidar = None
+        self.robot_command_queue = []
         self.joy_state = Joy()
 
         self.cmd_vel_sub = self.create_subscription(
@@ -90,6 +91,12 @@ def __init__(self):
             self.joy_cb,
             qos_profile)
 
+        self.command_sub = self.create_subscription(
+            String,
+            'command',
+            self.command_cb,
+            qos_profile)
+
         self.timer = self.create_timer(0.1, self.timer_callback)
         self.timer_lidar = self.create_timer(0.5, self.timer_callback_lidar)
 
@@ -111,12 +118,23 @@ def cmd_vel_cb(self, msg):
     def joy_cb(self, msg):
         self.joy_state = msg
 
+    def command_cb(self, msg):
+        self.get_logger().info(f"Received command: {msg.data}")
+        self.robot_command_queue.append(msg)
+
     def joy_cmd(self):
         if self.robot_cmd_vel:
             self.get_logger().info("Attack!")
             self.conn.data_channel.send(self.robot_cmd_vel)
             self.robot_cmd_vel = None
 
+        if len(self.robot_command_queue) > 0:
+            msg = self.robot_command_queue[0]
+            robot_cmd = gen_command(ROBOT_CMD[msg.data])
+            self._logger.info(f"Sending command: {ROBOT_CMD[msg.data]}")
+            self.conn.data_channel.send(robot_cmd)
+            self.robot_command_queue.pop(0)
+
         if self.joy_state.buttons and self.joy_state.buttons[1]:
             self.get_logger().info("Stand down")
             stand_down_cmd = gen_command(ROBOT_CMD["StandDown"])
@@ -126,6 +144,8 @@ def joy_cmd(self):
             self.get_logger().info("Stand up")
             stand_up_cmd = gen_command(ROBOT_CMD["StandUp"])
             self.conn.data_channel.send(stand_up_cmd)
+            balance_stand_cmd = gen_command(ROBOT_CMD['BalanceStand'])
+            self.conn.data_channel.send(balance_stand_cmd)
 
     def on_validated(self):
         for topic in RTC_TOPIC.values():

go2_robot_sdk/launch/robot.launch.py

@@ -166,6 +166,10 @@ def generate_launch_description():
             executable='go2_driver_node',
             parameters=[{'robot_ip': robot_ip, 'token': robot_token}],
             ),
+        Node(
+            package='go2_robot_sdk',
+            executable='go2_proc_text',
+            ),
         Node(
             package='ros2_go2_video',
             executable='ros2_go2_video',

go2_robot_sdk/scripts/go2_proc_text.py

@@ -0,0 +1,92 @@
+# Copyright (c) 2024, RoboVerse community
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice, this
+#    list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+#    this list of conditions and the following disclaimer in the documentation
+#    and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+
+from std_msgs.msg import String
+import os
+
+
+class Go2ProcText(Node):
+
+    def __init__(self):
+        super().__init__('go2_proc_text')
+
+        text_file_name = '/tmp/robot_cmd'
+
+        # remove all contents of the file
+        with open(text_file_name, 'w') as file:
+            pass
+
+        # touch the file
+        os.utime(text_file_name, None)
+
+        self.tmp_file = open(text_file_name, 'r')
+
+        qos_profile = QoSProfile(depth=10)
+        self.publisher_ = self.create_publisher(String, 'command', qos_profile)
+
+        self.text_read_timer = self.create_timer(0.2, self.update)
+
+    def update(self):
+        line = self.tmp_file.readline()
+        self.get_logger().info('I heard: "%s"' % line)
+
+        # TODO: replace this with chatGPT to process natural language
+        # and translate it into a queue of robot actions
+        if "sit" in line.lower():
+            msg = String()
+            msg.data = "Sit"
+            self.publisher_.publish(msg)
+
+        if "stand" in line.lower():
+            msg = String()
+            msg.data = "RecoveryStand"
+            self.publisher_.publish(msg)
+            msg.data = "StandUp"
+            self.publisher_.publish(msg)
+            msg.data = "BalanceStand"
+            self.publisher_.publish(msg)
+
+        if "shake hand" in line.lower():
+            msg = String()
+            msg.data = "Hello"
+            self.publisher_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    go2_proc_text = Go2ProcText()
+
+    rclpy.spin(go2_proc_text)
+
+    go2_proc_text.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

go2_robot_sdk/setup.py

@@ -56,6 +56,7 @@
     entry_points={
         'console_scripts': [
             'go2_driver_node = go2_robot_sdk.go2_driver_node:main',
+            'go2_proc_text = scripts.go2_proc_text:main',
         ],
     },
 )