go2py bridge major update

Go2Py/robot/interface.py

@@ -6,8 +6,9 @@ import numpy.linalg as LA
 from scipy.spatial.transform import Rotation as R
 
 from cyclonedds.domain import DomainParticipant
-from go2py_common.msg.dds_ import Go2pyLowCmd_
+from go2py_messages.msg.dds_ import Go2pyLowCmd_
-from unitree_go.msg.dds_ import LowState_
+from go2py_messages.msg.dds_ import Go2pyHighCmd_
+from go2py_messages.msg.dds_ import Go2pyState_
 from cyclonedds.topic import Topic
 from cyclonedds.pub import DataWriter
 
@@ -31,8 +32,6 @@ class GO2Real():
     ):
         assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
         self.mode = mode
-        if self.mode == 'highlevel':
-            raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')
         self.simulated = False
         self.prestanding_q = np.array([ 0.0,  1.26186061, -2.5,
                                     0.0,  1.25883281, -2.5,
@@ -50,56 +49,58 @@ class GO2Real():
                                      0.0,  0.75422204, -1.53229916])
         self.latest_command_stamp = time.time()
         self.highcmd_topic_name = "rt/go2/twist_cmd"
-        self.lowcmd_topic_name = "rt/go2/lowcmd"
+        self.lowcmd_topic_name = "rt/go2py/low_cmd"
-        self.lowstate_topic_name = "rt/lowstate"
+        self.highcmd_topic_name = "rt/go2py/high_cmd"
+        self.lowstate_topic_name = "rt/go2py/state"
 
         self.participant = DomainParticipant()
-        self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_)
+        self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, Go2pyState_)
-        self.lowstate_reader = DataReader(self.participant, self.lowstate_topic)
+        self.state_reader = DataReader(self.participant, self.lowstate_topic)
         
         self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_)
         self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic)
 
+        self.highcmd_topic = Topic(self.participant, self.highcmd_topic_name, Go2pyHighCmd_)
+        self.highcmd_writer = DataWriter(self.participant, self.highcmd_topic)
+        self.vx_max = vx_max
+        self.vy_max = vy_max
+        self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2])
+        self.ωz_max = ωz_max
+        self.ωz_min = -ωz_max
+        
         self.state = None
-        self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode]
+        self.setCommands = {'lowlevel':self.setCommandsLow,
-        self.lowstate_thread = Thread(target = self.lowstate_update)
+                            'highlevel':self.setCommandsHigh}[self.mode]
+        self.state_thread = Thread(target = self.state_update)
         self.running = True
-        self.lowstate_thread.start()
+        self.state_thread.start()
 
-
+    def state_update(self):
-    def lowstate_update(self):
         """
         Retrieve the state of the robot
         """
         while self.running:
-            for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=1.)):
+            for msg in self.state_reader.take_iter(timeout=duration(milliseconds=1.)):
                 self.state = msg
 
     def getIMU(self):
-        accel = self.state.imu_state.accelerometer
+        accel = self.state.accel
-        gyro = self.state.imu_state.gyroscope
+        gyro = self.state.gyro
-        quat = self.state.imu_state.quaternion
+        quat = self.state.quat
-        rpy = self.state.imu_state.rpy
+        temp = self.state.imu_temp
-        temp = self.state.imu_state.temperature
+        return {'accel':accel, 'gyro':gyro, 'quat':quat, 'temp':temp}
-        return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
 
     def getFootContacts(self):
         """Returns the raw foot contact forces"""
-        footContacts = self.state.foot_force 
+        footContacts = self.state.contact 
-        return np.array(footContacts)
+        return footContacts
 
     def getJointStates(self):
         """Returns the joint angles (q) and velocities (dq) of the robot"""
-        motor_state = np.array([[self.state.motor_state[i].q,
+        return {'q':self.state.q, 
-                                 self.state.motor_state[i].dq,
+                'dq':self.state.dq,
-                                 self.state.motor_state[i].ddq,
+                'tau_est':self.state.tau,
-                                 self.state.motor_state[i].tau_est,
+                'temperature':self.state.motor_temp}
-                                 self.state.motor_state[i].temperature] for i in range(12)])
-        return {'q':motor_state[:,0], 
-                'dq':motor_state[:,1],
-                'ddq':motor_state[:,2],
-                'tau_est':motor_state[:,3],
-                'temperature':motor_state[:,4]}
 
     def getRemoteState(self):
         """A method to get the state of the wireless remote control. 
@@ -149,18 +150,17 @@ class GO2Real():
 
     def getBatteryState(self):
         """Returns the battery percentage of the robot"""
-        batteryState = self.state.bms_state
+        return self.state.soc
-        return batteryState.soc
 
     def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
         self.cmd_watchdog_timer = time.time()
         _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)
-        self.highcmd.header.stamp = self.get_clock().now().to_msg()
+        highcmd = Go2pyHighCmd_(
-        self.highcmd.header.frame_id = "base_link"
+            _v_x,
-        self.highcmd.twist.linear.x = _v_x
+            _v_y,
-        self.highcmd.twist.linear.y = _v_y
+            _ω_z
-        self.highcmd.twist.angular.z = _ω_z
+        )
-        self.highcmd_publisher.publish(self.highcmd)
+        self.highcmd_writer.write(highcmd)
 
     def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
         assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
@@ -177,11 +177,6 @@ class GO2Real():
     def close(self):
         self.running = False
 
-    def check_calf_collision(self, q):
-        self.pin_robot.update(q)
-        in_collision = self.pin_robot.check_calf_collision(q)
-        return in_collision
-
     def clip_velocity(self, v_x, v_y, ω_z):
         _v = np.array([[v_x], [v_y]])
         _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]
@@ -198,6 +193,6 @@ class GO2Real():
 
     def getGravityInBody(self):
         q = self.getIMU()['quat']
-        R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
+        R = Rotation.from_quat(q).as_matrix()
         g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
         return g_in_body

Go2Py/robot/interface/ros2.py

@@ -1,313 +0,0 @@
-import struct
-import threading
-import time
-import numpy as np
-import numpy.linalg as LA
-from scipy.spatial.transform import Rotation as R
-import rclpy
-import tf2_ros
-from rclpy.node import Node
-from rclpy.qos import QoSProfile
-from rclpy.executors import MultiThreadedExecutor
-from geometry_msgs.msg import TransformStamped
-from Go2Py.joy import xKeySwitch, xRockerBtn
-from geometry_msgs.msg import TwistStamped
-from unitree_go.msg import LowState, Go2pyLowCmd
-from nav_msgs.msg import Odometry   
-from scipy.spatial.transform import Rotation
-import tf2_ros
-import numpy as np
-from scipy.spatial.transform import Rotation as R
-
-
-
-def ros2_init(args=None):
-    rclpy.init(args=args)
-
-
-def ros2_close():
-    rclpy.shutdown()
-
-class ROS2ExecutorManager:
-    """A class to manage the ROS2 executor. It allows to add nodes and start the executor in a separate thread."""
-    def __init__(self):
-        self.executor = MultiThreadedExecutor()
-        self.nodes = []
-        self.executor_thread = None
-
-    def add_node(self, node: Node):
-        """Add a new node to the executor."""
-        self.nodes.append(node)
-        self.executor.add_node(node)
-
-    def _run_executor(self):
-        try:
-            self.executor.spin()
-        except KeyboardInterrupt:
-            pass
-        finally:
-            self.terminate()
-
-    def start(self):
-        """Start spinning the nodes in a separate thread."""
-        self.executor_thread = threading.Thread(target=self._run_executor)
-        self.executor_thread.start()
-
-    def terminate(self):
-        """Terminate all nodes and shutdown rclpy."""
-        for node in self.nodes:
-            node.destroy_node()
-        rclpy.shutdown()
-        if self.executor_thread:
-            self.executor_thread.join()
-
-class GO2Real(Node):
-    def __init__(
-        self,
-        mode = 'highlevel', # 'highlevel' or 'lowlevel'
-        vx_max=0.5,
-        vy_max=0.4,
-        ωz_max=0.5,
-    ):
-        assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'"
-        self.simulated = False
-        self.prestanding_q = np.array([ 0.0,  1.26186061, -2.5,
-                                    0.0,  1.25883281, -2.5,
-                                    0.0,  1.27193761, -2.6,  
-                                    0.0,  1.27148342, -2.6])
-
-        self.sitting_q = np.array([-0.02495611,  1.26249647, -2.82826662,
-                                    0.04563564,  1.2505368 , -2.7933557 ,
-                                   -0.30623949,  1.28283751, -2.82314873,  
-                                    0.26400229,  1.29355574, -2.84276843])
-
-        self.standing_q = np.array([ 0.0,  0.77832842, -1.56065452,
-                                     0.0,  0.76754963, -1.56634164,
-                                     0.0,  0.76681757, -1.53601146,  
-                                     0.0,  0.75422204, -1.53229916])
-        self.latest_command_stamp = time.time()
-        self.mode = mode
-        self.node_name = "go2py_highlevel_subscriber"
-        self.highcmd_topic = "/go2/twist_cmd"
-        self.lowcmd_topic = "/go2/lowcmd"
-        self.joint_state_topic = "/go2/joint_states"
-        self.lowstate_topic = "/lowstate"
-        super().__init__(self.node_name)
-        
-        self.lowstate_subscriber = self.create_subscription(
-            LowState, self.lowstate_topic, self.lowstate_callback, 1
-        )
-        self.lowcmd_publisher = self.create_publisher(Go2pyLowCmd, self.lowcmd_topic, 1)
-
-        self.odometry_subscriber = self.create_subscription(
-            Odometry, "/utlidar/robot_odom", self.odom_callback, 1
-        )
-
-        self.highcmd_publisher = self.create_publisher(TwistStamped, self.highcmd_topic, 1)
-        self.highcmd = TwistStamped()
-        # create pinocchio robot
-        # self.pin_robot = PinRobot()
-
-        # for velocity clipping
-        self.vx_max = vx_max
-        self.vy_max = vy_max
-        self.P_v_max = np.diag([1 / self.vx_max**2, 1 / self.vy_max**2])
-        self.ωz_max = ωz_max
-        self.ωz_min = -ωz_max
-        self.running = True
-        self.setCommands = {'lowlevel':self.setCommandsLow,
-                            'highlevel':self.setCommandsHigh}[self.mode]
-        self.state = None
-
-    def lowstate_callback(self, msg):
-        """
-        Retrieve the state of the robot
-        """
-        self.state = msg
-
-    def odom_callback(self, msg):
-        """
-        Retrieve the odometry of the robot
-        """
-        self.odom = msg
-
-    def getOdometry(self):
-        """Returns the odometry of the robot"""
-        stamp = self.odom.header.stamp
-        position = np.array([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z])
-        orientation = np.array([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])
-        stamp_nanosec = stamp.sec + stamp.nanosec * 1e-9
-        return {'stamp_nanosec':stamp_nanosec, 'position':position, 'orientation':orientation}
-
-    def getIMU(self):
-        accel = self.state.imu_state.accelerometer
-        gyro = self.state.imu_state.gyroscope
-        quat = self.state.imu_state.quaternion
-        rpy = self.state.imu_state.rpy
-        temp = self.state.imu_state.temperature
-        # return accel, gyro, quat, temp
-        return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
-
-    def getFootContacts(self):
-        """Returns the raw foot contact forces"""
-        footContacts = self.state.foot_force
-        return np.array(footContacts)
-
-    def getJointStates(self):
-        """Returns the joint angles (q) and velocities (dq) of the robot"""
-        if self.state is None:
-            return None
-        motor_state = np.array([[self.state.motor_state[i].q,
-                                 self.state.motor_state[i].dq,
-                                 self.state.motor_state[i].ddq,
-                                 self.state.motor_state[i].tau_est,
-                                 self.state.motor_state[i].temperature] for i in range(12)])
-        return {'q':motor_state[:,0], 
-                'dq':motor_state[:,1],
-                'ddq':motor_state[:,2],
-                'tau_est':motor_state[:,3],
-                'temperature':motor_state[:,4]}
-
-    def getRemoteState(self):
-        """A method to get the state of the wireless remote control. 
-        Returns a xRockerBtn object: 
-        - head: [head1, head2]
-        - keySwitch: xKeySwitch object
-        - lx: float
-        - rx: float
-        - ry: float
-        - L2: float
-        - ly: float
-        """
-        wirelessRemote = self.state.wireless_remote[:24]
-
-        binary_data = bytes(wirelessRemote)
-
-        format_str = "<2BH5f"
-        data = struct.unpack(format_str, binary_data)
-
-        head = list(data[:2])
-        lx = data[3]
-        rx = data[4]
-        ry = data[5]
-        L2 = data[6]
-        ly = data[7]
-
-        _btn = bin(data[2])[2:].zfill(16)
-        btn = [int(char) for char in _btn]
-        btn.reverse()
-
-        keySwitch = xKeySwitch(*btn)
-        rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly)
-        return rockerBtn
-
-    def getCommandFromRemote(self):
-        """Do not use directly for control!!!"""
-        rockerBtn = self.getRemoteState()
-
-        lx = rockerBtn.lx
-        ly = rockerBtn.ly
-        rx = rockerBtn.rx
-
-        v_x = ly * self.vx_max
-        v_y = lx * self.vy_max
-        ω = rx * self.ωz_max
-        
-        return v_x, v_y, ω
-
-    def getBatteryState(self):
-        """Returns the battery percentage of the robot"""
-        batteryState = self.state.bms
-        return batteryState.SOC
-
-    def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
-        self.cmd_watchdog_timer = time.time()
-        _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)
-        self.highcmd.header.stamp = self.get_clock().now().to_msg()
-        self.highcmd.header.frame_id = "base_link"
-        self.highcmd.twist.linear.x = _v_x
-        self.highcmd.twist.linear.y = _v_y
-        self.highcmd.twist.angular.z = _ω_z
-        self.highcmd_publisher.publish(self.highcmd)
-
-    def setCommandsLow(self, q_des, dq_des, kp, kd, tau_ff):
-        # assert q_des.size == dq_des.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
-        lowcmd = Go2pyLowCmd()
-        lowcmd.q = q_des
-        lowcmd.dq = dq_des
-        lowcmd.kp = kp
-        lowcmd.kd = kd
-        lowcmd.tau = tau_ff
-        self.lowcmd_publisher.publish(lowcmd)
-        self.latest_command_stamp = time.time()
-
-
-    def close(self):
-        self.running = False
-        self.thread.join()
-        self.destroy_node()
-
-    def check_calf_collision(self, q):
-        self.pin_robot.update(q)
-        in_collision = self.pin_robot.check_calf_collision(q)
-        return in_collision
-
-    def clip_velocity(self, v_x, v_y, ω_z):
-        _v = np.array([[v_x], [v_y]])
-        _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]
-
-        if _scale > 1.0:
-            scale = 1.0 / _scale
-        else:
-            scale = 1.0
-
-        return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)
-
-    def overheat(self):
-        return False
-
-    def getGravityInBody(self):
-        q = self.getIMU()['quat']
-        R = Rotation.from_quat([q[1], q[2], q[3], q[0]]).as_matrix()
-        g_in_body = R.T@np.array([0.0, 0.0, -1.0]).reshape(3, 1)
-        return g_in_body
-
-class ROS2TFInterface(Node):
-
-    def __init__(self, parent_name, child_name, node_name):
-        super().__init__(f'{node_name}_tf2_listener')
-        self.parent_name = parent_name
-        self.child_name = child_name
-        self.tfBuffer = tf2_ros.Buffer()
-        self.listener = tf2_ros.TransformListener(self.tfBuffer, self)
-        self.T = None
-        self.stamp = None
-        self.running = True
-        self.thread = threading.Thread(target=self.update_loop)
-        self.thread.start()
-        self.trans = None
-
-    def update_loop(self):
-        while self.running:
-            try:
-                self.trans = self.tfBuffer.lookup_transform(self.parent_name, self.child_name, rclpy.time.Time(), rclpy.time.Duration(seconds=0.1))
-            except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
-                pass
-            time.sleep(0.01)    
-
-    def get_pose(self):
-        if self.trans is None:
-            return None
-        else:
-            translation = [self.trans.transform.translation.x, self.trans.transform.translation.y, self.trans.transform.translation.z]
-            rotation = [self.trans.transform.rotation.x, self.trans.transform.rotation.y, self.trans.transform.rotation.z, self.trans.transform.rotation.w]
-            self.T = np.eye(4)
-            self.T[0:3, 0:3] = R.from_quat(rotation).as_matrix()
-            self.T[:3, 3] = translation
-            self.stamp = self.trans.header.stamp.nanosec * 1e-9 + self.trans.header.stamp.sec
-            return self.T
-
-    def close(self):
-        self.running = False
-        self.thread.join()  
-        self.destroy_node()

Makefile

@@ -1,3 +1,6 @@
+messages:
+	@cd scripts && ./make_msgs.sh 
+
 realsense:
 	@cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense .
 

dds_messages/idls/BmsState_.idl

@@ -1,49 +0,0 @@
-// generated from rosidl_generator_dds_idl/resource/idl.idl.em
-// with input from unitree_go:msg/BmsState.idl
-// generated code does not contain a copyright notice
-
-#ifndef __unitree_go__msg__bms_state__idl__
-#define __unitree_go__msg__bms_state__idl__
-module unitree_go {
-
-module msg {
-
-module dds_ {
-
-
-struct BmsState_ {
-octet version_high;    //电池版本
-octet version_low;     //电池版本
-
-// 0：SAFE,（未开启电池）
-// 1：WAKE_UP,（唤醒事件）
-
-// 6：PRECHG, （电池预冲电中）
-// 7：CHG, （电池正常充电中）
-// 8：DCHG, （电池正常放电中）
-// 9：SELF_DCHG, （电池自放电中）
-
-// 11：ALARM, （电池存在警告）
-// 12：RESET_ALARM, （等待按键复位警告中）
-// 13：AUTO_RECOVERY （复位中）
-octet status;          //电池状态信息。
-  
-octet soc;             //电池电量信息：（类型：uint8_t）(范围1% - 100%)
-long current;          //充放电信息：（正：代表充电，负代表放电）可按照实际数值显示
-unsigned short cycle;  //充电循环次数
-octet bq_ntc[2];       //电池内部两个NTC的温度（int8_t）（范围：-100 - 150）。  0- BAT1; 1- BAT2
-
-octet mcu_ntc[2];      //电池NTC数组：0 - RES，1 - MOS （int8_t）（范围：-100 - 150）。
-
-unsigned short cell_vol[15];      //电池内部15节电池的电压。
-
-
-};
-
-
-};  // module dds_
-
-};  // module msg
-
-};  // module unitree_go
-#endif  // __unitree_go__msg__bms_state__idl__

dds_messages/idls/IMUState_.idl

@@ -1,33 +0,0 @@
-// generated from rosidl_generator_dds_idl/resource/idl.idl.em
-// with input from unitree_go:msg/IMUState.idl
-// generated code does not contain a copyright notice
-
-#ifndef __unitree_go__msg__imu_state__idl__
-#define __unitree_go__msg__imu_state__idl__
-module unitree_go {
-
-module msg {
-
-module dds_ {
-
-
-struct IMUState_ {
-float quaternion[4];    //四元数数据
-
-float gyroscope[3];     //角速度信息（0 -> x ,0 -> y ,0 -> z）
-
-float accelerometer[3]; //加速度信息（0 -> x ,0 -> y ,0 -> z）
-
-float rpy[3];           //欧拉角信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。（数组：0-roll（翻滚角），1-pitch（俯仰角），2-yaw（偏航角））。
-
-octet temperature;      //IMU 温度信息（摄氏度）。
-
-};
-
-
-};  // module dds_
-
-};  // module msg
-
-};  // module unitree_go
-#endif  // __unitree_go__msg__imu_state__idl__

dds_messages/idls/LowState_.idl

@@ -1,70 +0,0 @@
-// generated from rosidl_generator_dds_idl/resource/idl.idl.em
-// with input from unitree_go:msg/LowState.idl
-// generated code does not contain a copyright notice
-#include "BmsState_.idl"
-#include "IMUState_.idl"
-#include "MotorState_.idl"
-
-#ifndef __unitree_go__msg__low_state__idl__
-#define __unitree_go__msg__low_state__idl__
-module unitree_go {
-
-module msg {
-
-module dds_ {
-
-
-struct LowState_ {
-octet head[2];               //帧头，数据校验用（0xFE,0xEF）。
-
-octet level_flag;            //沿用的，但是目前不用。
-octet frame_reserve;         //沿用的，但是目前不用。
-unsigned long sn[2];         //已经改为文件存储形式，目前没用。
-unsigned long version[2];    //沿用的，但是目前不用。
-unsigned short bandwidth;    //沿用的，但是目前不用。。
-  
-unitree_go::msg::dds_::IMUState_ imu_state;           //IMU数据信息。
-
-// FR_0 -> 0 , FR_1 -> 1  , FR_2 -> 2   电机顺序，目前只用12电机，后面保留。
-// FL_0 -> 3 , FL_1 -> 4  , FL_2 -> 5
-// RR_0 -> 6 , RR_1 -> 7  , RR_2 -> 8
-// RL_0 -> 9 , RL_1 -> 10 , RL_2 -> 11
-unitree_go::msg::dds_::MotorState_ motor_state[20];   //电机总数据。
-unitree_go::msg::dds_::BmsState_ bms_state;           //电池总数据。
-
-short foot_force[4];        //足端力（范围0-4095），可按照实际数值显示。（数组：0-FR，1-FL，2-RR, 3-RL）
-short foot_force_est[4];    //沿用的，但是目前不用。
-
-unsigned long tick;         //1ms计时用，按照1ms递增。
-octet wireless_remote[40];  //遥控器原始数据。
-
-//&0x80 -  电机               超时标志          1-超时   0-正常
-//&0x40 -  小Mcu              超时标志          1-超时   0-正常
-//&0x20 -  遥控器             超时标志          1-超时   0-正常
-//&0x10 -  电池               超时标志          1-超时   0-正常
-
-//&0x04 -  自动充电           自动充电状态标志  1-不充电           0-充电
-//&0x02 -  板载电流错误标志   错误标志          1-板载电流异常     0-正常
-//&0x01 -  运控命令超时       超时标志          1-超时             0-正常
-octet bit_flag;             //各个组件状态显示
-  
-float adc_reel;             //卷线器电流（范围：0 - 3A）。
-octet temperature_ntc1;    	//主板中心温度值（范围：-20 - 100℃）。
-octet temperature_ntc2;    	//自动充电温度（范围：-20 - 100℃）。
-float power_v;      				//此电压值为主板电压 -> 电池电压 。
-float power_a;      				//此电流值为主板电流值 -> 电机电流。
-  
-unsigned short fan_frequency[4];    //风扇转速（目前可按照实际数值显示0-10000）。（0-左后转速 , 1-右后转速，2-前转速，单位转/分钟）（堵转检测：3-&0x01：左后堵转 , &0x02：右后堵转，&0x04：前堵转）
-
-unsigned long reserve;   //保留位。
-unsigned long crc;       //数据CRC校验用。
-
-};
-
-
-};  // module dds_
-
-};  // module msg
-
-};  // module unitree_go
-#endif  // __unitree_go__msg__low_state__idl__

dds_messages/idls/MotorState_.idl

@@ -1,37 +0,0 @@
-// generated from rosidl_generator_dds_idl/resource/idl.idl.em
-// with input from unitree_go:msg/MotorState.idl
-// generated code does not contain a copyright notice
-
-#ifndef __unitree_go__msg__motor_state__idl__
-#define __unitree_go__msg__motor_state__idl__
-module unitree_go {
-
-module msg {
-
-module dds_ {
-
-
-struct MotorState_ {
-octet mode;     //电机控制模式（Foc模式（工作模式）-> 0x01 ，stop模式（待机模式）-> 0x00。）
-float q;        //关机反馈位置信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。
-float dq;       //关节反馈速度
-float ddq;      //关节反馈加速度
-float tau_est;  //关节反馈力矩
-  
-float q_raw;    //沿用的，但是目前不用。
-float dq_raw;   //沿用的，但是目前不用。
-float ddq_raw;  //沿用的，但是目前不用。
-octet temperature;          //电机温度信息：类型：int8_t ，可按照实际数值显示（范围：-100 - 150）。
-unsigned long lost;         //电机丢包信息：可按照实际数值显示（范围：0-9999999999）。
-unsigned long reserve[2];   //当前电机通信频率+电机错误标志位：（数组：0-电机错误标志位（范围：0-255，可按照实际数值显示），1-当前电机通信频率（范围：0-800，可按照实际数值显示））
-
-
-};
-
-
-};  // module dds_
-
-};  // module msg
-
-};  // module unitree_go
-#endif  // __unitree_go__msg__motor_state__idl__

deploy/ros2_nodes/go2py_bridge/CMakeLists.txt

@@ -21,7 +21,7 @@ link_directories(src)
 
 set (
   DEPENDENCY_LIST
-  go2py_common
+  go2py_messages
   unitree_go
   unitree_api
   rclcpp
@@ -39,7 +39,7 @@ set (
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(unitree_go REQUIRED)
-find_package(go2py_common REQUIRED)
+find_package(go2py_messages REQUIRED)
 find_package(unitree_api REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)

deploy/ros2_nodes/go2py_bridge/package.xml

@@ -10,7 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>unitree_go</depend>
-  <depend>go2py_common</depend>
+  <depend>go2py_messages</depend>
   <depend>unitree_api</depend>
   <depend>rclcpp</depend>
   <depend>std_msgs</depend>

deploy/ros2_nodes/go2py_bridge/src/bridge.cpp

@@ -1,8 +1,10 @@
 #include "unitree_go/msg/low_state.hpp"
 #include "unitree_go/msg/imu_state.hpp"
 #include "unitree_go/msg/motor_state.hpp"
-#include "go2py_common/msg/go2py_status.hpp"
+#include "go2py_messages/msg/go2py_status.hpp"
-#include "go2py_common/msg/go2py_low_cmd.hpp"
+#include "go2py_messages/msg/go2py_low_cmd.hpp"
+#include "go2py_messages/msg/go2py_high_cmd.hpp"
+#include "go2py_messages/msg/go2py_state.hpp"
 
 #include "unitree_go/msg/low_cmd.hpp"
 #include "unitree_go/msg/motor_cmd.hpp"
@@ -31,72 +33,69 @@ class Custom: public rclcpp::Node
     public:
         Custom() : Node("go2py_bridge_node")
         {
+            // Standard ROS2 Topics
             watchdog_timer = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&Custom::watchdog, this));
             pub_odom = this->create_publisher<nav_msgs::msg::Odometry>("/go2/odom", 1);
             pub_imu  = this->create_publisher<sensor_msgs::msg::Imu>("/go2/imu", 1);
             pub_joint  = this->create_publisher<sensor_msgs::msg::JointState>("/go2/joint_states", 1);
-            sub_twist = this->create_subscription<geometry_msgs::msg::TwistStamped>("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1));
+            nav2_twist_subr = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
-            nav2_twist = this->create_subscription<geometry_msgs::msg::Twist>("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
             
-            // Go2 highlevel subscriber and publishers
+            //Unitree Topics
-            // the state_suber is set to subscribe "sportmodestate" topic
-            highstate_suber = this->create_subscription<unitree_go::msg::SportModeState>(
-                "sportmodestate", 10, std::bind(&Custom::highstate_callback, this, std::placeholders::_1));
-            // the req_puber is set to subscribe "/api/sport/request" topic with dt
-            highreq_puber = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 1);
-            
-            //Go2 lowlevel interface
             init_lowcmd();
-            lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
+            unitree_lowstate_suber = this->create_subscription<unitree_go::msg::LowState>(
-            "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
+            "lowstate", 1, std::bind(&Custom::unitree_lowstate_callback, this, std::placeholders::_1));
-            
-            lowcmd_suber = this->create_subscription<go2py_common::msg::Go2pyLowCmd>(
-            "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
 
+            // the req_puber is set to subscribe "/api/sport/request" topic with dt
+            unitree_highreq_puber = this->create_publisher<unitree_api::msg::Request>("/api/sport/request", 1);
             lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/lowcmd", 1);
-            api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 1);
-            status_publisher = this->create_publisher<go2py_common::msg::Go2pyStatus>("/go2py/status", 1);
             
+            // Go2py topics
+            go2py_low_cmd_suber = this->create_subscription<go2py_messages::msg::Go2pyLowCmd>(
+            "/go2py/low_cmd", 1, std::bind(&Custom::go2py_low_cmd_callback, this, std::placeholders::_1));
+            go2py_high_cmd_suber = this->create_subscription<go2py_messages::msg::Go2pyHighCmd>(
+            "/go2py/high_cmd", 1, std::bind(&Custom::go2py_high_cmd_callback, this, std::placeholders::_1));
+            go2py_state_puber = this->create_publisher<go2py_messages::msg::Go2pyState>("/go2py/state", 1);
+            status_publisher = this->create_publisher<go2py_messages::msg::Go2pyStatus>("/go2py/status", 1);
+            api_publisher = this->create_publisher<unitree_api::msg::Request>("/api/robot_state/request", 1);
         }
     private:
         rclcpp::TimerBase::SharedPtr watchdog_timer;
         void watchdog();
-        // Highlevel twist command through standard ROS2 message type
+        void init_lowcmd();
-        void twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
+        int Estop = 0;
-        void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+        int sport_mode = 1;
-        geometry_msgs::msg::TwistStamped twist_cmd;
+        xRockerBtnDataStruct _keyData;
-        geometry_msgs::msg::Twist twist;
-        rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub_twist;
-        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr nav2_twist;
-        uint64_t last_highcmd_stamp = 0;
 
-        // ROS2 standard joint state, IMU, and odometry publishers
+        // Standard ROS2
+        geometry_msgs::msg::TwistStamped twist_cmd;
+        void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr nav2_twist_subr;        
         rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub_imu;
         rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_joint;
         rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odom;
         
-        // Highlevel interface
+        // Unitree Interface
-        void highstate_callback(unitree_go::msg::SportModeState::SharedPtr data);
+        unitree_api::msg::Request highreq;
-        rclcpp::Subscription<unitree_go::msg::SportModeState>::SharedPtr highstate_suber;
+        unitree_go::msg::LowCmd lowcmd_msg;
-        rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr highreq_puber;
-        unitree_api::msg::Request highreq; // Unitree Go2 ROS2 request message
         SportClient sport_req;
+        void unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data);
+        rclcpp::Subscription<unitree_go::msg::LowState>::SharedPtr unitree_lowstate_suber;
+        rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr unitree_highreq_puber;
+        rclcpp::Publisher<unitree_go::msg::LowCmd>::SharedPtr lowcmd_puber;
+        rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
 
         // Lowlevel interface
-        void lowstate_callback(unitree_go::msg::LowState::SharedPtr data);
+        void go2py_low_cmd_callback(go2py_messages::msg::Go2pyLowCmd::SharedPtr data);
-        void lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data);
+        void go2py_high_cmd_callback(go2py_messages::msg::Go2pyHighCmd::SharedPtr data);
-        rclcpp::Subscription<unitree_go::msg::LowState>::SharedPtr lowstate_suber;
+        rclcpp::Subscription<go2py_messages::msg::Go2pyLowCmd>::SharedPtr go2py_low_cmd_suber;
-        rclcpp::Subscription<go2py_common::msg::Go2pyLowCmd>::SharedPtr lowcmd_suber;
+        rclcpp::Subscription<go2py_messages::msg::Go2pyHighCmd>::SharedPtr go2py_high_cmd_suber;
-        // A struct to store the highlevel states for later use
+        rclcpp::Publisher<go2py_messages::msg::Go2pyState>::SharedPtr go2py_state_puber;
-        rclcpp::Publisher<unitree_go::msg::LowCmd>::SharedPtr lowcmd_puber;
+        rclcpp::Publisher<go2py_messages::msg::Go2pyStatus>::SharedPtr status_publisher;  
-        unitree_go::msg::LowCmd lowcmd_msg;
-        uint64_t last_lowcmd_stamp = 0;
-        xRockerBtnDataStruct _keyData;
-        rclcpp::Publisher<unitree_api::msg::Request>::SharedPtr api_publisher;
-        rclcpp::Publisher<go2py_common::msg::Go2pyStatus>::SharedPtr status_publisher;
 
-        void init_lowcmd()
+};
-        {
+
+void Custom::init_lowcmd()
+{
 
     for (int i = 0; i < 20; i++)
     {
@@ -107,36 +106,14 @@ class Custom: public rclcpp::Node
         lowcmd_msg.motor_cmd[i].kd = 0;
         lowcmd_msg.motor_cmd[i].tau = 0;
     }
-        }
+}
 
-        struct {
+void Custom::unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
-            float px;
-            float py;
-            float height;
-            float roll;
-            float pitch;
-            float yaw;
-            float imu_temp;
-            float wx;
-            float wy;
-            float wz; 
-            float ax;
-            float qy;
-            float az; 
-            float vx;
-            float vy;
-            float foot_force[4];
-        }highstate;     
-        int Estop = 0;
-        int sport_mode = 1;
-};
-
-void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
 {
-    // std::cout << data->tick << std::endl;
     sensor_msgs::msg::Imu imu;
     sensor_msgs::msg::JointState joint_state;
     nav_msgs::msg::Odometry odom_state;
+    go2py_messages::msg::Go2pyState go2py_state; 
 
     // Load the IMU message
     imu.header.stamp=rclcpp::Clock().now();
@@ -151,6 +128,20 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
     imu.angular_velocity.x = data->imu_state.gyroscope[0];
     imu.angular_velocity.y = data->imu_state.gyroscope[1];
     imu.angular_velocity.z = data->imu_state.gyroscope[2];
+    
+    go2py_state.time_sec = imu.header.stamp.sec;
+    go2py_state.time_nsec = imu.header.stamp.nanosec;
+    go2py_state.quat[0] = imu.orientation.x;
+    go2py_state.quat[1] = imu.orientation.y;
+    go2py_state.quat[2] = imu.orientation.z;
+    go2py_state.quat[3] = imu.orientation.w;
+    for(int i=0; i<3; i++)
+    {
+        go2py_state.accel[i] = data->imu_state.accelerometer[i];
+        go2py_state.gyro[i] = data->imu_state.gyroscope[i];
+    }
+    go2py_state.imu_temp = data->imu_state.temperature;
+
     // Load the joint state messages
     joint_state.header.stamp = imu.header.stamp;
     joint_state.header.frame_id = "trunk";
@@ -175,10 +166,20 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
         joint_state.position.push_back(data->motor_state[i].q);
         joint_state.velocity.push_back(data->motor_state[i].dq);
         joint_state.effort.push_back(data->motor_state[i].tau_est);
+        go2py_state.q[i]=data->motor_state[i].q;
+        go2py_state.dq[i]=data->motor_state[i].dq;
+        go2py_state.tau[i]=data->motor_state[i].tau_est;
+        go2py_state.motor_temp[i] = data->motor_state[i].temperature;
     }
+    for(int i=0; i<4; i++)
+        go2py_state.contact[i]=data->foot_force[i];
 
+    for(int i=0; i<40; i++)
+        go2py_state.wireless_remote[i]=data->wireless_remote[i];
+    go2py_state.soc = data->bms_state.soc;
     pub_joint->publish(joint_state);
     pub_imu->publish(imu);
+    go2py_state_puber->publish(go2py_state);
     // Check for emergency stop
     memcpy(&_keyData, &data->wireless_remote[0], 40);
     if (_keyData.btn.components.R2 == 1 && _keyData.btn.components.L2 == 1)
@@ -212,83 +213,29 @@ void Custom::lowstate_callback(unitree_go::msg::LowState::SharedPtr data)
     // std::cout << "Estop: " << Estop << std::endl;
 }
 
-void Custom::highstate_callback(unitree_go::msg::SportModeState::SharedPtr data)
-{
-    highstate.px = data->position[0];
-    highstate.py = data->position[1];
-    highstate.roll = data->imu_state.rpy[0];
-    highstate.pitch = data->imu_state.rpy[1];
-    highstate.yaw = data->imu_state.rpy[2];
-    for(int i=0; i<4; i++)
-    {
-        highstate.foot_force[i]=data->foot_force[i];
-    }
-    highstate.height = data->body_height;
-    
-    nav_msgs::msg::Odometry odom_state;
-    // odometry states published by the onboard high-level controller
-    odom_state.header.stamp = rclcpp::Clock().now();
-    odom_state.header.frame_id = "odom";
-    odom_state.child_frame_id = "base_link";
-    odom_state.pose.pose.position.x = data->position[0];
-    odom_state.pose.pose.position.y = data->position[1];
-    odom_state.pose.pose.position.z = data->body_height;
-
-
-    odom_state.twist.twist.linear.x = data->velocity[0];
-    odom_state.twist.twist.linear.y = data->velocity[1];
-    odom_state.twist.twist.linear.z = data->velocity[2];
-    odom_state.twist.twist.angular.z= data->yaw_speed;
-    double position_R = 0.05;
-    double orientation_R = 0.005;
-    odom_state.pose.covariance = {
-        position_R, 0, 0, 0, 0, 0,  // Covariance for position x
-        0, position_R, 0, 0, 0, 0,  // Covariance for position y
-        0, 0, position_R, 0, 0, 0,  // Covariance for position z
-        0, 0, 0, orientation_R, 0, 0,  // Covariance for orientation roll
-        0, 0, 0, 0, orientation_R, 0,  // Covariance for orientation pitch
-        0, 0, 0, 0, 0, orientation_R   // Covariance for orientation yaw    
-    };
-
-    odom_state.twist.covariance = {
-        position_R, 0, 0, 0, 0, 0,  // Covariance for position x
-        0, position_R, 0, 0, 0, 0,  // Covariance for position y
-        0, 0, position_R, 0, 0, 0,  // Covariance for position z
-        0, 0, 0, orientation_R, 0, 0,  // Covariance for orientation roll
-        0, 0, 0, 0, orientation_R, 0,  // Covariance for orientation pitch
-        0, 0, 0, 0, 0, orientation_R   // Covariance for orientation yaw    
-    };
-    
-    pub_odom->publish(odom_state);
-}
-
-void Custom::twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
-{
-    auto stamp_now = std::chrono::high_resolution_clock::now();
-    last_highcmd_stamp = std::chrono::duration_cast<std::chrono::microseconds>(stamp_now.time_since_epoch()).count();
-    if(Estop == 0)
-        sport_req.Move(highreq, msg->twist.linear.x, msg->twist.linear.y, msg->twist.angular.z);
-    else
-        sport_req.Damp(highreq);
-    
-    // Publish request messages with desired body velocity
-    highreq_puber->publish(highreq);
-}
-
 void Custom::nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
-    auto stamp_now = std::chrono::high_resolution_clock::now();
-    last_highcmd_stamp = std::chrono::duration_cast<std::chrono::microseconds>(stamp_now.time_since_epoch()).count();
     if(Estop == 0)
         sport_req.Move(highreq, msg->linear.x, msg->linear.y, msg->angular.z);
     else
         sport_req.Damp(highreq);
     
     // Publish request messages with desired body velocity
-    highreq_puber->publish(highreq);
+    unitree_highreq_puber->publish(highreq);
 }
 
-void Custom::lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data)
+void Custom::go2py_high_cmd_callback(go2py_messages::msg::Go2pyHighCmd::SharedPtr data)
+{
+    if(Estop == 0)
+        sport_req.Move(highreq, data->vx, data->vy, data->wz);
+    else
+        sport_req.Damp(highreq);
+    
+    // Publish request messages with desired body velocity
+    unitree_highreq_puber->publish(highreq);   
+}
+
+void Custom::go2py_low_cmd_callback(go2py_messages::msg::Go2pyLowCmd::SharedPtr data)
 {   
     if(Estop == 0)
     {
@@ -321,7 +268,7 @@ void Custom::watchdog()
     if(Estop == 1)
     {
         sport_req.Damp(highreq);    
-        highreq_puber->publish(highreq);
+        unitree_highreq_puber->publish(highreq);
         for(int i=0; i<12; i++)
         {
             lowcmd_msg.motor_cmd[i].q =   0.;   // Taregt angular(rad)
@@ -329,12 +276,11 @@ void Custom::watchdog()
             lowcmd_msg.motor_cmd[i].dq =  0.;  // Taregt angular velocity(rad/ss)
             lowcmd_msg.motor_cmd[i].kd =  3;  // Poinstion(rad) control kd gain
             lowcmd_msg.motor_cmd[i].tau = 0.; // Feedforward toque 1N.m
-            get_crc(lowcmd_msg); //Compute the CRC and load it into the message
-            lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message
         }
+        get_crc(lowcmd_msg); //Compute the CRC and load it into the message
         lowcmd_puber->publish(lowcmd_msg);
     }
-    auto msg = std::make_shared<go2py_common::msg::Go2pyStatus>();
+    auto msg = std::make_shared<go2py_messages::msg::Go2pyStatus>();
     msg->estop = Estop;
     msg->sport_mode = sport_mode;
     status_publisher->publish(*msg);

deploy/ros2_nodes/go2py_common/msg/AudioData.msg

@@ -1,2 +0,0 @@
-uint64 time_frame
-uint8[] data

deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg

@@ -1,2 +0,0 @@
-uint8 off
-uint8[3] reserve

deploy/ros2_nodes/go2py_common/msg/BmsState.msg

@@ -1,9 +0,0 @@
-uint8 version_high
-uint8 version_low
-uint8 status
-uint8 soc
-int32 current
-uint16 cycle
-int8[2] bq_ntc
-int8[2] mcu_ntc
-uint16[15] cell_vol

deploy/ros2_nodes/go2py_common/msg/Error.msg

@@ -1,2 +0,0 @@
-uint32 source
-uint32 state

deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg

@@ -1,4 +0,0 @@
-uint64 time_frame
-uint8[] video720p
-uint8[] video360p
-uint8[] video180p 

deploy/ros2_nodes/go2py_common/msg/HeightMap.msg

@@ -1,15 +0,0 @@
-# Header
-float64 stamp         # timestamp
-string frame_id      # world frame id
-
-# Map info
-float32 resolution     # The map resolution [m/cell]
-uint32 width  # Map width along x-axis [cells]
-uint32 height # Map height alonge y-axis [cells]
-float32[2] origin      # Map frame origin xy-position [m], the xyz-axis direction of map frame is aligned with the world frame
-
-# Map data, in x-major order, starting with [0,0], ending with [width, height]
-# For a cell whose 2d-array-index is [ix, iy]，
-#    its position in world frame is: [ix * resolution + origin[0], iy * resolution + origin[1]]
-#    its cell value is: data[width * iy + ix]
-float32[] data

deploy/ros2_nodes/go2py_common/msg/IMUState.msg

@@ -1,5 +0,0 @@
-float32[4] quaternion
-float32[3] gyroscope
-float32[3] accelerometer
-float32[3] rpy
-int8 temperature

deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg

@@ -1,3 +0,0 @@
-uint8 mode
-uint8 value
-uint8[2] reserve

deploy/ros2_nodes/go2py_common/msg/LidarState.msg

@@ -1,17 +0,0 @@
-float64 stamp
-string firmware_version
-string software_version
-string sdk_version
-float32 sys_rotation_speed
-float32 com_rotation_speed
-uint8 error_state
-float32 cloud_frequency
-float32 cloud_packet_loss_rate
-uint32 cloud_size
-uint32 cloud_scan_num
-float32 imu_frequency
-float32 imu_packet_loss_rate
-float32[3] imu_rpy
-float64 serial_recv_stamp
-uint32 serial_buffer_size
-uint32 serial_buffer_read

deploy/ros2_nodes/go2py_common/msg/LowCmd.msg

@@ -1,15 +0,0 @@
-uint8[2] head
-uint8 level_flag
-uint8 frame_reserve
-uint32[2] sn
-uint32[2] version
-uint16 bandwidth
-MotorCmd[20] motor_cmd
-BmsCmd bms_cmd
-uint8[40] wireless_remote
-uint8[12] led
-uint8[2] fan
-uint8 gpio
-uint32 reserve
-uint32 crc
-

deploy/ros2_nodes/go2py_common/msg/LowState.msg

@@ -1,22 +0,0 @@
-uint8[2] head
-uint8 level_flag
-uint8 frame_reserve
-uint32[2] sn
-uint32[2] version
-uint16 bandwidth
-IMUState imu_state
-MotorState[20] motor_state
-BmsState bms_state
-int16[4] foot_force
-int16[4] foot_force_est
-uint32 tick
-uint8[40] wireless_remote
-uint8 bit_flag
-float32 adc_reel
-int8 temperature_ntc1
-int8 temperature_ntc2
-float32 power_v
-float32 power_a
-uint16[4] fan_frequency
-uint32 reserve
-uint32 crc

deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg

@@ -1,7 +0,0 @@
-uint8 mode
-float32 q
-float32 dq
-float32 tau
-float32 kp
-float32 kd
-uint32[3] reserve

deploy/ros2_nodes/go2py_common/msg/MotorState.msg

@@ -1,11 +0,0 @@
-uint8 mode
-float32 q
-float32 dq
-float32 ddq
-float32 tau_est
-float32 q_raw
-float32 dq_raw
-float32 ddq_raw
-int8 temperature
-uint32 lost
-uint32[2] reserve

deploy/ros2_nodes/go2py_common/msg/PathPoint.msg

@@ -1,7 +0,0 @@
-float32 t_from_start
-float32 x
-float32 y
-float32 yaw
-float32 vx
-float32 vy
-float32 vyaw

deploy/ros2_nodes/go2py_common/msg/Req.msg

@@ -1,2 +0,0 @@
-string uuid
-string body

deploy/ros2_nodes/go2py_common/msg/Res.msg

@@ -1,3 +0,0 @@
-string uuid
-uint8[] data
-string body

deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg

@@ -1,13 +0,0 @@
-uint8 mode
-uint8 gait_type
-uint8 speed_level
-float32 foot_raise_height
-float32 body_height
-float32[2] position
-float32[3] euler
-float32[2] velocity
-float32 yaw_speed
-BmsCmd bms_cmd
-PathPoint[30] path_point
-
-

deploy/ros2_nodes/go2py_common/msg/SportModeState.msg

@@ -1,17 +0,0 @@
-TimeSpec stamp
-uint32 error_code
-IMUState imu_state
-uint8 mode
-float32 progress
-uint8 gait_type
-float32 foot_raise_height
-float32[3] position
-float32 body_height
-float32[3] velocity
-float32 yaw_speed
-float32[4] range_obstacle
-int16[4] foot_force
-float32[12] foot_position_body
-float32[12] foot_speed_body
-
-

deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg

@@ -1,5 +0,0 @@
-# Time indicates a specific point in time, relative to a clock's 0 point.
-# The seconds component, valid over all int32 values.
-int32 sec
-# The nanoseconds component, valid in the range [0, 10e9).
-uint32 nanosec

deploy/ros2_nodes/go2py_common/msg/UwbState.msg

@@ -1,19 +0,0 @@
-uint8[2] version
-uint8 channel
-uint8 joy_mode
-float32 orientation_est
-float32 pitch_est
-float32 distance_est
-float32 yaw_est
-float32 tag_roll
-float32 tag_pitch
-float32 tag_yaw
-float32 base_roll
-float32 base_pitch
-float32 base_yaw
-float32[2] joystick
-uint8 error_state
-uint8 buttons
-uint8 enabled_from_app
-
-

deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg

@@ -1 +0,0 @@
-uint8 enabled

deploy/ros2_nodes/go2py_common/msg/WirelessController.msg

@@ -1,5 +0,0 @@
-float32 lx
-float32 ly
-float32 rx
-float32 ry
-uint16 keys

deploy/ros2_nodes/go2py_messages/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(go2py_common)
+project(go2py_messages)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
@@ -26,29 +26,9 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(rosidl_generator_dds_idl REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-        "msg/AudioData.msg"
-        "msg/BmsCmd.msg"
-        "msg/BmsState.msg"
-        "msg/Error.msg"
-        "msg/Go2FrontVideoData.msg"
-        "msg/HeightMap.msg"
-        "msg/IMUState.msg"
-        "msg/InterfaceConfig.msg"
-        "msg/LidarState.msg"
-        "msg/LowCmd.msg"
-        "msg/LowState.msg"
-        "msg/MotorCmd.msg"
-        "msg/MotorState.msg"
-        "msg/PathPoint.msg"
-        "msg/Req.msg"
-        "msg/Res.msg"
-        "msg/SportModeCmd.msg"
-        "msg/SportModeState.msg"
-        "msg/TimeSpec.msg"
-        "msg/UwbState.msg"
-        "msg/UwbSwitch.msg"
-        "msg/WirelessController.msg"
         "msg/Go2pyLowCmd.msg"
+        "msg/Go2pyHighCmd.msg"
+        "msg/Go2pyState.msg"
         "msg/Go2pyStatus.msg"
   DEPENDENCIES geometry_msgs
 )

deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg

@@ -0,0 +1,3 @@
+float32 vx
+float32 vy
+float32 wz

deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg

@@ -0,0 +1,13 @@
+uint64      time_sec
+uint64      time_nsec
+float32[12] q
+float32[12] dq   
+float32[12] tau
+float32[4]  contact
+float32[3]  accel    
+float32[3]  gyro    
+float32[4]  quat
+float32     imu_temp    
+float32[12] motor_temp
+uint8[40] wireless_remote
+uint8 soc

deploy/ros2_nodes/go2py_messages/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>go2py_common</name>
+  <name>go2py_messages</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="czk@todo.todo">czk</maintainer>

examples/00-robot-interface copy.ipynb

@@ -7,16 +7,6 @@
     "# Lowlevel Control"
    ]
   },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import unitree_go\n",
-    "unitree_go.__file__"
-   ]
-  },
   {
    "cell_type": "code",
    "execution_count": null,
@@ -65,14 +55,36 @@
   },
   {
    "cell_type": "code",
-   "execution_count": null,
+   "execution_count": 2,
    "metadata": {},
-   "outputs": [],
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
+      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
+     ]
+    }
+   ],
    "source": [
     "from Go2Py.robot.interface.dds import GO2Real\n",
     "import time\n",
     "robot = GO2Real(mode='highlevel')"
    ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "import time\n",
+    "for i in range(200):\n",
+    "    robot.setCommandsHigh(0,0,0.2)\n",
+    "    time.sleep(0.01)    "
+   ]
   }
  ],
  "metadata": {

examples/01-robot-interface-ROS2.ipynb

@@ -1,156 +0,0 @@
-{
- "cells": [
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# Lowlevel Control"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
-    "import time\n",
-    "ros2_init()\n",
-    "robot = GO2Real(mode='lowlevel')\n",
-    "ros2_exec_manager = ROS2ExecutorManager()\n",
-    "ros2_exec_manager.add_node(robot)\n",
-    "ros2_exec_manager.start()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import time\n",
-    "import numpy as np\n",
-    "qs = []\n",
-    "dqs = []\n",
-    "taus = []\n",
-    "stamps = []\n",
-    "for i in range(5000):\n",
-    "    time.sleep(0.01)\n",
-    "    state = robot.getJointStates()\n",
-    "    q = state['q']\n",
-    "    dq = state['dq']\n",
-    "    tau = state['tau_est']\n",
-    "    stamp = time.time()\n",
-    "    qs.append(q)\n",
-    "    dqs.append(dq)\n",
-    "    taus.append(tau)\n",
-    "    stamps.append(stamp)\n",
-    "\n",
-    "q=np.array(qs)\n",
-    "dq=np.array(dqs)\n",
-    "tau=np.array(taus)\n",
-    "stamp=np.array(stamps)"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import pickle\n",
-    "with open('go2_rotation.pkl', 'wb') as f:\n",
-    "    pickle.dump(\n",
-    "        {\n",
-    "            'q':q,\n",
-    "            'dq':dq, \n",
-    "            'tau':tau, \n",
-    "            'stamp':stamp\n",
-    "        }\n",
-    "        ,f\n",
-    "    )"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import numpy as np\n",
-    "import time\n",
-    "for i in range(10000):\n",
-    "    q = np.zeros(12) \n",
-    "    dq = np.zeros(12)\n",
-    "    kp = np.zeros(12)\n",
-    "    kd = np.zeros(12)\n",
-    "    tau = np.zeros(12)\n",
-    "    tau[0] = -0.8\n",
-    "    robot.setCommands(q, dq, kp, kd, tau)\n",
-    "    time.sleep(0.01)    "
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "import matplotlib.pyplot as plt\n",
-    "plt.plot(taus)"
-   ]
-  },
-  {
-   "cell_type": "markdown",
-   "metadata": {},
-   "source": [
-    "# Highlevel Control"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
-    "import time\n",
-    "ros2_init()\n",
-    "robot = GO2Real(mode='highlevel')\n",
-    "ros2_exec_manager = ROS2ExecutorManager()\n",
-    "ros2_exec_manager.add_node(robot)\n",
-    "ros2_exec_manager.start()"
-   ]
-  },
-  {
-   "cell_type": "code",
-   "execution_count": null,
-   "metadata": {},
-   "outputs": [],
-   "source": [
-    "robot.getJointStates()"
-   ]
-  }
- ],
- "metadata": {
-  "kernelspec": {
-   "display_name": "Python 3",
-   "language": "python",
-   "name": "python3"
-  },
-  "language_info": {
-   "codemirror_mode": {
-    "name": "ipython",
-    "version": 3
-   },
-   "file_extension": ".py",
-   "mimetype": "text/x-python",
-   "name": "python",
-   "nbconvert_exporter": "python",
-   "pygments_lexer": "ipython3",
-   "version": "3.8.18"
-  }
- },
- "nbformat": 4,
- "nbformat_minor": 4
-}

examples/01-ros2-tools.ipynb

@@ -0,0 +1,111 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Lowlevel Control"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "import time\n",
+    "robot = GO2Real(mode='lowlevel')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "robot.getJointStates()"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "import time\n",
+    "for i in range(1000000):\n",
+    "    q = np.zeros(12) \n",
+    "    dq = np.zeros(12)\n",
+    "    kp = np.ones(12)*0.0\n",
+    "    kd = np.ones(12)*0.0\n",
+    "    tau = np.zeros(12)\n",
+    "    tau[0] = 0.0\n",
+    "    robot.setCommands(q, dq, kp, kd, tau)\n",
+    "    time.sleep(0.01)    "
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Highlevel Control"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 2,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n",
+      "Hello from the pygame community. https://www.pygame.org/contribute.html\n"
+     ]
+    }
+   ],
+   "source": [
+    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "import time\n",
+    "robot = GO2Real(mode='highlevel')"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import numpy as np\n",
+    "import time\n",
+    "for i in range(200):\n",
+    "    robot.setCommandsHigh(0,0,0.2)\n",
+    "    time.sleep(0.01)    "
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python 3",
+   "language": "python",
+   "name": "python3"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.8.18"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 4
+}

go2py_common/.idlpy_manifest

@@ -1,3 +0,0 @@
-Go2pyLowCmd
-msg
-

go2py_common/msg/.idlpy_manifest

@@ -1,3 +0,0 @@
-Go2pyLowCmd
-dds_
-

go2py_common/msg/__init__.py

@@ -1,9 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: go2py_common.msg
-
-"""
-
-from . import dds_
-__all__ = ["dds_", ]

go2py_common/msg/dds_/.idlpy_manifest

@@ -1,3 +0,0 @@
-Go2pyLowCmd
-
-Go2pyLowCmd_

go2py_common/msg/dds_/__init__.py

@@ -1,9 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: go2py_common.msg.dds_
-
-"""
-
-from ._Go2pyLowCmd import Go2pyLowCmd_
-__all__ = ["Go2pyLowCmd_", ]

go2py_messages/.idlpy_manifest

@@ -0,0 +1,11 @@
+Go2pyHighCmd
+msg
+
+
+Go2pyLowCmd
+msg
+
+
+Go2pyState
+msg
+

go2py_messages/__init__.py

@@ -1,7 +1,7 @@
 """
   Generated by Eclipse Cyclone DDS idlc Python Backend
   Cyclone DDS IDL version: v0.11.0
-  Module: go2py_common
+  Module: go2py_messages
 
 """
 

go2py_messages/msg/.idlpy_manifest

@@ -0,0 +1,11 @@
+Go2pyHighCmd
+dds_
+
+
+Go2pyLowCmd
+dds_
+
+
+Go2pyState
+dds_
+

go2py_messages/msg/__init__.py

@@ -1,7 +1,7 @@
 """
   Generated by Eclipse Cyclone DDS idlc Python Backend
   Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg
+  Module: go2py_messages.msg
 
 """
 

go2py_messages/msg/dds_/.idlpy_manifest

@@ -0,0 +1,11 @@
+Go2pyHighCmd
+
+Go2pyHighCmd_
+
+Go2pyLowCmd
+
+Go2pyLowCmd_
+
+Go2pyState
+
+Go2pyState_

go2py_messages/msg/dds_/_Go2pyHighCmd.py

@@ -1,8 +1,8 @@
 """
   Generated by Eclipse Cyclone DDS idlc Python Backend
   Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg.dds_
+  Module: go2py_messages.msg.dds_
-  IDL file: IMUState_.idl
+  IDL file: Go2pyHighCmd.idl
 
 """
 
@@ -15,17 +15,15 @@ import cyclonedds.idl.annotations as annotate
 import cyclonedds.idl.types as types
 
 # root module import for resolving types
-import unitree_go
+import go2py_messages
 
 
 @dataclass
 @annotate.final
 @annotate.autoid("sequential")
-class IMUState_(idl.IdlStruct, typename="unitree_go.msg.dds_.IMUState_"):
+class Go2pyHighCmd_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyHighCmd_"):
-    quaternion: types.array[types.float32, 4]
+    vx: types.float32
-    gyroscope: types.array[types.float32, 3]
+    vy: types.float32
-    accelerometer: types.array[types.float32, 3]
+    vz: types.float32
-    rpy: types.array[types.float32, 3]
-    temperature: types.uint8
 
 

go2py_messages/msg/dds_/_Go2pyLowCmd.py

@@ -1,7 +1,7 @@
 """
   Generated by Eclipse Cyclone DDS idlc Python Backend
   Cyclone DDS IDL version: v0.11.0
-  Module: go2py_common.msg.dds_
+  Module: go2py_messages.msg.dds_
   IDL file: Go2pyLowCmd.idl
 
 """
@@ -15,13 +15,13 @@ import cyclonedds.idl.annotations as annotate
 import cyclonedds.idl.types as types
 
 # root module import for resolving types
-import go2py_common
+import go2py_messages
 
 
 @dataclass
 @annotate.final
 @annotate.autoid("sequential")
-class Go2pyLowCmd_(idl.IdlStruct, typename="go2py_common.msg.dds_.Go2pyLowCmd_"):
+class Go2pyLowCmd_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyLowCmd_"):
     q: types.array[types.float32, 12]
     dq: types.array[types.float32, 12]
     kp: types.array[types.float32, 12]

go2py_messages/msg/dds_/_Go2pyState.py

@@ -0,0 +1,39 @@
+"""
+  Generated by Eclipse Cyclone DDS idlc Python Backend
+  Cyclone DDS IDL version: v0.11.0
+  Module: go2py_messages.msg.dds_
+  IDL file: Go2pyState.idl
+
+"""
+
+from enum import auto
+from typing import TYPE_CHECKING, Optional
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+# root module import for resolving types
+import go2py_messages
+
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class Go2pyState_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyState_"):
+    time_sec: types.uint64
+    time_nsec: types.uint64
+    q: types.array[types.float32, 12]
+    dq: types.array[types.float32, 12]
+    tau: types.array[types.float32, 12]
+    contact: types.array[types.float32, 4]
+    accel: types.array[types.float32, 3]
+    gyro: types.array[types.float32, 3]
+    quat: types.array[types.float32, 4]
+    imu_temp: types.float32
+    motor_temp: types.array[types.float32, 12]
+    wireless_remote: types.array[types.uint8, 40]
+    soc: types.uint8
+
+

go2py_messages/msg/dds_/__init__.py

@@ -0,0 +1,11 @@
+"""
+  Generated by Eclipse Cyclone DDS idlc Python Backend
+  Cyclone DDS IDL version: v0.11.0
+  Module: go2py_messages.msg.dds_
+
+"""
+
+from ._Go2pyHighCmd import Go2pyHighCmd_
+from ._Go2pyLowCmd import Go2pyLowCmd_
+from ._Go2pyState import Go2pyState_
+__all__ = ["Go2pyHighCmd_", "Go2pyLowCmd_", "Go2pyState_", ]

scripts/dds_messages/Go2pyHighCmd.idl

@@ -0,0 +1,11 @@
+module go2py_messages {
+  module msg {
+    module dds_{
+      @final struct Go2pyHighCmd_ {
+        float vx;
+        float vy;
+        float vz;
+      };
+    };
+  };
+};

scripts/dds_messages/Go2pyLowCmd.idl

@@ -1,4 +1,4 @@
-module go2py_common {
+module go2py_messages {
   module msg {
     module dds_{
       @final struct Go2pyLowCmd_ {

scripts/dds_messages/Go2pyState.idl

@@ -0,0 +1,21 @@
+module go2py_messages {
+  module msg {
+    module dds_{
+      @final struct Go2pyState_ {
+      Unsigned long long time_sec;
+      Unsigned long long time_nsec;
+      float q[12];
+      float dq[12];   
+      float tau[12];
+      float contact[4];
+      float accel[3];    
+      float gyro[3];    
+      float quat[4];
+      float imu_temp;    
+      float motor_temp[12];
+      octet wireless_remote[40];
+      octet soc;
+      };
+    };
+  };
+};

scripts/make_msgs.sh

@@ -5,14 +5,14 @@ NC='\033[0m' # No Color
 
 echo -e "${GREEN} Starting DDS type generation...${NC}"
 # Clean
-rm -r ../unitree_go
+# rm -r ../unitree_go
-rm -r ../go2py_common
+rm -r ../go2py_messages
 
 # Make
-for file in idls/*.idl
+for file in dds_messages/*.idl
 do
     echo "Processing $file file..."
     idlc -l py $file
 done
-mv unitree_go ../
+# mv unitree_go ../
-mv go2py_common ../
+mv go2py_messages ../

unitree_go/.idlpy_manifest

@@ -1,15 +0,0 @@
-BmsState_
-msg
-
-
-IMUState_
-msg
-
-
-LowState_
-msg
-
-
-MotorState_
-msg
-

unitree_go/__init__.py

@@ -1,9 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go
-
-"""
-
-from . import msg
-__all__ = ["msg", ]

unitree_go/msg/.idlpy_manifest

@@ -1,15 +0,0 @@
-BmsState_
-dds_
-
-
-IMUState_
-dds_
-
-
-LowState_
-dds_
-
-
-MotorState_
-dds_
-

unitree_go/msg/dds_/.idlpy_manifest

@@ -1,15 +0,0 @@
-BmsState_
-
-BmsState_
-
-IMUState_
-
-IMUState_
-
-LowState_
-
-LowState_
-
-MotorState_
-
-MotorState_

unitree_go/msg/dds_/_BmsState_.py

@@ -1,35 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg.dds_
-  IDL file: BmsState_.idl
-
-"""
-
-from enum import auto
-from typing import TYPE_CHECKING, Optional
-from dataclasses import dataclass
-
-import cyclonedds.idl as idl
-import cyclonedds.idl.annotations as annotate
-import cyclonedds.idl.types as types
-
-# root module import for resolving types
-import unitree_go
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class BmsState_(idl.IdlStruct, typename="unitree_go.msg.dds_.BmsState_"):
-    version_high: types.uint8
-    version_low: types.uint8
-    status: types.uint8
-    soc: types.uint8
-    current: types.int32
-    cycle: types.uint16
-    bq_ntc: types.array[types.uint8, 2]
-    mcu_ntc: types.array[types.uint8, 2]
-    cell_vol: types.array[types.uint16, 15]
-
-

unitree_go/msg/dds_/_LowState_.py

@@ -1,48 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg.dds_
-  IDL file: LowState_.idl
-
-"""
-
-from enum import auto
-from typing import TYPE_CHECKING, Optional
-from dataclasses import dataclass
-
-import cyclonedds.idl as idl
-import cyclonedds.idl.annotations as annotate
-import cyclonedds.idl.types as types
-
-# root module import for resolving types
-import unitree_go
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class LowState_(idl.IdlStruct, typename="unitree_go.msg.dds_.LowState_"):
-    head: types.array[types.uint8, 2]
-    level_flag: types.uint8
-    frame_reserve: types.uint8
-    sn: types.array[types.uint32, 2]
-    version: types.array[types.uint32, 2]
-    bandwidth: types.uint16
-    imu_state: 'unitree_go.msg.dds_.IMUState_'
-    motor_state: types.array['unitree_go.msg.dds_.MotorState_', 20]
-    bms_state: 'unitree_go.msg.dds_.BmsState_'
-    foot_force: types.array[types.int16, 4]
-    foot_force_est: types.array[types.int16, 4]
-    tick: types.uint32
-    wireless_remote: types.array[types.uint8, 40]
-    bit_flag: types.uint8
-    adc_reel: types.float32
-    temperature_ntc1: types.uint8
-    temperature_ntc2: types.uint8
-    power_v: types.float32
-    power_a: types.float32
-    fan_frequency: types.array[types.uint16, 4]
-    reserve: types.uint32
-    crc: types.uint32
-
-

unitree_go/msg/dds_/_MotorState_.py

@@ -1,37 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg.dds_
-  IDL file: MotorState_.idl
-
-"""
-
-from enum import auto
-from typing import TYPE_CHECKING, Optional
-from dataclasses import dataclass
-
-import cyclonedds.idl as idl
-import cyclonedds.idl.annotations as annotate
-import cyclonedds.idl.types as types
-
-# root module import for resolving types
-import unitree_go
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class MotorState_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorState_"):
-    mode: types.uint8
-    q: types.float32
-    dq: types.float32
-    ddq: types.float32
-    tau_est: types.float32
-    q_raw: types.float32
-    dq_raw: types.float32
-    ddq_raw: types.float32
-    temperature: types.uint8
-    lost: types.uint32
-    reserve: types.array[types.uint32, 2]
-
-

unitree_go/msg/dds_/__init__.py

@@ -1,12 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: unitree_go.msg.dds_
-
-"""
-
-from ._BmsState_ import BmsState_
-from ._IMUState_ import IMUState_
-from ._LowState_ import LowState_
-from ._MotorState_ import MotorState_
-__all__ = ["BmsState_", "IMUState_", "LowState_", "MotorState_", ]