add euler pose command

.conda-env

@@ -0,0 +1 @@
+ros

example/wireless_controller/wireless_controller_g1.py

@@ -0,0 +1,62 @@
+import time
+import sys
+from unitree_sdk2py.core.channel import ChannelPublisher, ChannelFactoryInitialize
+
+from unitree_sdk2py.idl.default import unitree_go_msg_dds__WirelessController_
+from unitree_sdk2py.idl.unitree_go.msg.dds_ import WirelessController_
+
+
+key_state = [
+    ["R1", 0],
+    ["L1", 0],
+    ["start", 0],
+    ["select", 0],
+    ["R2", 0],
+    ["L2", 0],
+    ["F1", 0],
+    ["F2", 0],
+    ["A", 0],
+    ["B", 0],
+    ["X", 0],
+    ["Y", 0],
+    ["up", 0],
+    ["right", 0],
+    ["down", 0],
+    ["left", 0],
+]
+
+
+def encode_keys(keys):
+    key_map = {key[0]: i for i, key in enumerate(key_state)}
+    encoded_value = 0
+
+    for key in keys:
+        if key in key_map:
+            encoded_value |= 1 << key_map[key]
+        else:
+            print(f"Warning: {key} is not a valid key")
+
+    return encoded_value
+
+
+if __name__ == "__main__":
+    ChannelFactoryInitialize(0, sys.argv[1])
+
+    pub = ChannelPublisher("rt/wirelesscontroller", WirelessController_)
+    pub.Init()
+    smp = unitree_go_msg_dds__WirelessController_()
+
+    smp.ly = 0.5
+    for i in range(100):
+        pub.Write(smp, timeout=1.0)
+        time.sleep(0.01)
+
+    smp.ly = 0
+    pub.Write(smp)
+
+    # smp.keys = encode_keys(["L1", "B"])
+    # for i in range(3):
+    #     pub.Write(smp, timeout=1)
+    #     time.sleep(0.1)
+
+    pub.Close()

unitree_sdk2py/b2/sport/sport_api.py

@@ -34,6 +34,7 @@
 ROBOT_SPORT_API_ID_CLASSICWALK       = 1049
 ROBOT_SPORT_API_ID_FASTWALK          = 1050
 ROBOT_SPORT_API_ID_FREEEULER         = 1051
+ROBOT_SPORT_API_ID_EULER             = 1007
 
 """
 " error code

unitree_sdk2py/b2/sport/sport_client.py

@@ -57,13 +57,23 @@ def Init(self):
         self._RegistApi(ROBOT_SPORT_API_ID_CLASSICWALK, 0)
         self._RegistApi(ROBOT_SPORT_API_ID_FASTWALK, 0)
         self._RegistApi(ROBOT_SPORT_API_ID_FREEEULER, 0)
+        self._RegistApi(ROBOT_SPORT_API_ID_EULER, 0)
 
     def Damp(self):
         p = {}
         parameter = json.dumps(p)
         code, data = self._Call(ROBOT_SPORT_API_ID_DAMP, parameter)
         return code
 
+    def Euler(self, roll: float, pitch: float, yaw: float):  
+        p = {}  
+        p["r"] = roll  
+        p["p"] = pitch  
+        p["y"] = yaw  
+        parameter = json.dumps(p)
+        code = self._CallNoReply(ROBOT_SPORT_API_ID_EULER, parameter)  
+        return code        
+
     def BalanceStand(self):
         p = {}
         parameter = json.dumps(p)

unitree_sdk2py/core/channel.py

@@ -28,11 +28,13 @@
 """
 " class Channel
 """
+
+
 class Channel:
-
     """
     " internal class __Reader
     """
+
     class __Reader:
         def __init__(self):
             self.__reader = None
@@ -41,8 +43,15 @@ def __init__(self):
             self.__queueEnable = False
             self.__threadEvent = None
             self.__threadReader = None
-
-        def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, handler: Callable = None, queueLen: int = 0):
+
+        def Init(
+            self,
+            participant: DomainParticipant,
+            topic: Topic,
+            qos: Qos = None,
+            handler: Callable = None,
+            queueLen: int = 0,
+        ):
             if handler is None:
                 self.__reader = DataReader(participant, topic, qos)
             else:
@@ -51,9 +60,18 @@ def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None, ha
                     self.__queueEnable = True
                     self.__queue = BQueue(queueLen)
                     self.__threadEvent = Event()
-                    self.__threadReader = Thread(target=self.__ChannelReaderThreadFunc, name="ch_reader", daemon=True)
+                    self.__threadReader = Thread(
+                        target=self.__ChannelReaderThreadFunc,
+                        name="ch_reader",
+                        daemon=True,
+                    )
                     self.__threadReader.start()
-                self.__reader = DataReader(participant, topic, qos, Listener(on_data_available=self.__OnDataAvailable))
+                self.__reader = DataReader(
+                    participant,
+                    topic,
+                    qos,
+                    Listener(on_data_available=self.__OnDataAvailable),
+                )
 
         def Read(self, timeout: float = None):
             sample = None
@@ -98,7 +116,7 @@ def __OnDataAvailable(self, reader: DataReader):
             if samples is None:
                 return
 
-            # check invalid sample        
+            # check invalid sample
             sample = samples[0]
             if isinstance(sample, InvalidSample):
                 return
@@ -118,13 +136,19 @@ def __ChannelReaderThreadFunc(self):
     """
     " internal class __Writer
     """
+
     class __Writer:
         def __init__(self):
             self.__writer = None
             self.__publication_matched_count = 0
-        
+
         def Init(self, participant: DomainParticipant, topic: Topic, qos: Qos = None):
-            self.__writer = DataWriter(participant, topic, qos, Listener(on_publication_matched=self.__OnPublicationMatched))
+            self.__writer = DataWriter(
+                participant,
+                topic,
+                qos,
+                Listener(on_publication_matched=self.__OnPublicationMatched),
+            )
             time.sleep(0.2)
 
         def Write(self, sample: Any, timeout: float = None):
@@ -137,7 +161,7 @@ def Write(self, sample: Any, timeout: float = None):
             #   print(time.time())
 
             # check waitsec
-            if timeout is not None and waitsec <= 0.0:
+            if timeout is not None and waitsec <= -0.1:
                 return False
 
             try:
@@ -150,17 +174,20 @@ def Write(self, sample: Any, timeout: float = None):
                 return False
 
             return True
-        
+
         def Close(self):
             if self.__writer is not None:
                 del self.__writer
-
-        def __OnPublicationMatched(self, writer: DataWriter, status: dds_c_t.publication_matched_status):
-            self.__publication_matched_count = status.current_count
 
+        def __OnPublicationMatched(
+            self, writer: DataWriter, status: dds_c_t.publication_matched_status
+        ):
+            self.__publication_matched_count = status.current_count
 
     # channel __init__
-    def __init__(self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None):
+    def __init__(
+        self, participant: DomainParticipant, name: str, type: Any, qos: Qos = None
+    ):
         self.__reader = self.__Reader()
         self.__writer = self.__Writer()
         self.__participant = participant
@@ -171,7 +198,7 @@ def SetWriter(self, qos: Qos = None):
 
     def SetReader(self, qos: Qos = None, handler: Callable = None, queueLen: int = 0):
         self.__reader.Init(self.__participant, self.__topic, qos, handler, queueLen)
-        
+
     def Write(self, sample: Any, timeout: float = None):
         return self.__writer.Write(sample, timeout)
 
@@ -188,6 +215,8 @@ def CloseWriter(self):
 """
 " class ChannelFactory
 """
+
+
 class ChannelFactory(Singleton):
     __domain = None
     __participant = None
@@ -244,7 +273,9 @@ def CreateSendChannel(self, name: str, type: Any):
         channel.SetWriter(None)
         return channel
 
-    def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queueLen: int = 0):
+    def CreateRecvChannel(
+        self, name: str, type: Any, handler: Callable = None, queueLen: int = 0
+    ):
         channel = self.CreateChannel(name, type)
         channel.SetReader(None, handler, queueLen)
         return channel
@@ -253,6 +284,8 @@ def CreateRecvChannel(self, name: str, type: Any, handler: Callable = None, queu
 """
 " class ChannelPublisher
 """
+
+
 class ChannelPublisher:
     def __init__(self, name: str, type: Any):
         factory = ChannelFactory()
@@ -271,9 +304,12 @@ def Close(self):
     def Write(self, sample: Any, timeout: float = None):
         return self.__channel.Write(sample, timeout)
 
+
 """
 " class ChannelSubscriber
 """
+
+
 class ChannelSubscriber:
     def __init__(self, name: str, type: Any):
         factory = ChannelFactory()
@@ -292,9 +328,12 @@ def Close(self):
     def Read(self, timeout: int = None):
         return self.__channel.Read(timeout)
 
+
 """
 " function ChannelFactoryInitialize. used to intialize channel everenment.
 """
+
+
 def ChannelFactoryInitialize(id: int = 0, networkInterface: str = None):
     factory = ChannelFactory()
     if not factory.Init(id, networkInterface):

unitree_sdk2py/rpc/client_stub.py

@@ -14,6 +14,8 @@
 """
 " class ClientStub
 """
+
+
 class ClientStub:
     def __init__(self, serviceName: str):
         self.__serviceName = serviceName
@@ -27,12 +29,17 @@ def Init(self):
         self.__futureQueue = RequestFutureQueue()
 
         # create channel
-        self.__sendChannel = factory.CreateSendChannel(GetClientChannelName(self.__serviceName, ChannelType.SEND), Request)
-        self.__recvChannel = factory.CreateRecvChannel(GetClientChannelName(self.__serviceName, ChannelType.RECV), Response,
-                                    self.__ResponseHandler,10)
+        self.__sendChannel = factory.CreateSendChannel(
+            GetClientChannelName(self.__serviceName, ChannelType.SEND), Request
+        )
+        self.__recvChannel = factory.CreateRecvChannel(
+            GetClientChannelName(self.__serviceName, ChannelType.RECV),
+            Response,
+            self.__ResponseHandler,
+            10,
+        )
         time.sleep(0.5)
 
-
     def Send(self, request: Request, timeout: float):
         if self.__sendChannel.Write(request, timeout):
             return True

unitree_sdk2py/test/client/vui_client_example.py

@@ -5,7 +5,7 @@
 from unitree_sdk2py.go2.vui.vui_client import VuiClient
 
 if __name__ == "__main__":
-    ChannelFactoryInitialize(0, "enp2s0")
+    ChannelFactoryInitialize(0, "en7")
 
     client = VuiClient()
     client.SetTimeout(3.0)
@@ -35,7 +35,7 @@
 
     print("#################SetBrightness 0####################")
 
-    code  = client.SetBrightness(0)
+    code = client.SetBrightness(0)
 
     if code != 0:
         print("set brightness error. code:", code)
@@ -66,7 +66,7 @@
 
     print("#################SetVolume 0####################")
 
-    code  = client.SetVolume(0)
+    code = client.SetVolume(0)
 
     if code != 0:
         print("set volume error. code:", code)