lowstate command interface added

.gitignore

@@ -160,9 +160,13 @@ cython_debug/
 #.idea/
 .vscode
 
-deploy/ros2_ws/log
+deploy/robot_ws/log
-deploy/ros2_ws/install
+deploy/robot_ws/install
-deploy/ros2_ws/build
+deploy/robot_ws/build
+
+deploy/dock_ws/log
+deploy/dock_ws/install
+deploy/dock_ws/build
 
 _isaac_sim
 .vscode

Go2Py/make_msgs.sh

@@ -20,6 +20,6 @@ do
     idlc -l ../../idl/libcycloneddsidlcxx.so.0.10.2 $file
 done
 cd ../..
-rm -r ../cpp_bridge/include/go2py
+# rm -r ../cpp_bridge/include/go2py
-mv msgs/cpp ../cpp_bridge/include/go2py
+# mv msgs/cpp ../cpp_bridge/include/go2py
 echo -e "${GREEN} Done with DDS type generation${NC}"

Go2Py/msgs/.idlpy_manifest

@@ -1,11 +0,0 @@
-Imu
-
-Imu
-
-LowCmd
-
-LowCmd
-
-LowState
-
-LowState

Go2Py/msgs/_Imu.py

@@ -1,31 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: msgs
-  IDL file: Imu.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 msgs
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class Imu(idl.IdlStruct, typename="msgs.Imu"):
-    q: types.array[types.float32, 4]
-    gyro: types.array[types.float32, 3]
-    accel: types.array[types.float32, 3]
-    rpy: types.array[types.float32, 3]
-    tmp: types.uint8
-
-

Go2Py/msgs/_LowCmd.py

@@ -1,32 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: msgs
-  IDL file: LowCmd.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 msgs
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class LowCmd(idl.IdlStruct, typename="msgs.LowCmd"):
-    q: types.array[types.float32, 12]
-    dq: types.array[types.float32, 12]
-    tau_ff: types.array[types.float32, 12]
-    kp: types.array[types.float32, 12]
-    kv: types.array[types.float32, 12]
-    e_stop: types.uint8
-
-

Go2Py/msgs/_LowState.py

@@ -1,39 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: msgs
-  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 msgs
-
-
-@dataclass
-@annotate.final
-@annotate.autoid("sequential")
-class LowState(idl.IdlStruct, typename="msgs.LowState"):
-    q: types.array[types.float32, 12]
-    dq: types.array[types.float32, 12]
-    ddq: types.array[types.float32, 12]
-    tau_est: types.array[types.float32, 12]
-    tmp: types.array[types.float32, 12]
-    contact: types.array[types.float32, 4]
-    quat: types.array[types.float32, 4]
-    gyro: types.array[types.float32, 3]
-    accel: types.array[types.float32, 3]
-    rpy: types.array[types.float32, 3]
-    imu_tmp: types.uint8
-    voltage: types.float32
-    current: types.float32
-
-

Go2Py/msgs/__init__.py

@@ -1,11 +0,0 @@
-"""
-  Generated by Eclipse Cyclone DDS idlc Python Backend
-  Cyclone DDS IDL version: v0.11.0
-  Module: msgs
-
-"""
-
-from ._Imu import Imu
-from ._LowCmd import LowCmd
-from ._LowState import LowState
-__all__ = ["Imu", "LowCmd", "LowState", ]

Go2Py/robot/interface/ros2.py

@@ -12,7 +12,7 @@ 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
+from unitree_go.msg import LowState, LowCmd
 from nav_msgs.msg import Odometry   
 
 
@@ -57,7 +57,6 @@ class ROS2ExecutorManager:
         if self.executor_thread:
             self.executor_thread.join()
 
-
 class GO2Real(Node):
     def __init__(
         self,
@@ -70,6 +69,7 @@ class GO2Real(Node):
         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)
@@ -77,6 +77,7 @@ class GO2Real(Node):
         self.lowstate_subscriber = self.create_subscription(
             LowState, self.lowstate_topic, self.lowstate_callback, 1
         )
+        self.lowcmd_publisher = self.create_publisher(LowCmd, self.lowcmd_topic, 1)
 
         self.odometry_subscriber = self.create_subscription(
             Odometry, "/utlidar/robot_odom", self.odom_callback, 1
@@ -94,6 +95,8 @@ class GO2Real(Node):
         self.ωz_max = ωz_max
         self.ωz_min = -ωz_max
         self.running = True
+        self.setCommands = {'lowstate':self.setCommandsLow,
+                            'highstate':self.setCommandsHigh}[self.mode]
 
     def lowstate_callback(self, msg):
         """
@@ -169,7 +172,6 @@ class GO2Real(Node):
 
         keySwitch = xKeySwitch(*btn)
         rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly)
-
         return rockerBtn
 
     def getCommandFromRemote(self):
@@ -191,18 +193,25 @@ class GO2Real(Node):
         batteryState = self.state.bms
         return batteryState.SOC
 
-    def setCommands(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
+    def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):
-        if self.mode == 'highlevel':
+        self.cmd_watchdog_timer = time.time()
-            self.cmd_watchdog_timer = time.time()
+        _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)
-            _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.stamp = self.get_clock().now().to_msg()
+        self.highcmd.header.frame_id = "base_link"
-            self.highcmd.header.frame_id = "base_link"
+        self.highcmd.twist.linear.x = _v_x
-            self.highcmd.twist.linear.x = _v_x
+        self.highcmd.twist.linear.y = _v_y
-            self.highcmd.twist.linear.y = _v_y
+        self.highcmd.twist.angular.z = _ω_z
-            self.highcmd.twist.angular.z = _ω_z
+        self.highcmd_publisher.publish(self.highcmd)
-            self.highcmd_publisher.publish(self.highcmd)
+
-        else:
+    def setCommandsLow(q, dq, kp, kd, tau_ff):
-            raise NotImplementedError("Low level control command is not implemented yet")
+        assert q.size == qd.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
+        lowcmd = LowCmd()
+        lowcmd.motor_cmd.q = q.tolist()
+        lowcmd.motor_cmd.dq = dq.tolist()
+        lowcmd.motor_cmd.kp = kp.tolist()
+        lowcmd.motor_cmd.kd = kd.tolist()
+        lowcmd.motor_cmd.tau_ff = tau_ff.tolist()
+        self.lowcmd_publisher.publish(lowcmd)
 
     def close(self):
         self.running = False