diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface.py
similarity index 67%
rename from Go2Py/robot/interface/dds.py
rename to Go2Py/robot/interface.py
index ea1fc1b..1cd7934 100644
--- a/Go2Py/robot/interface/dds.py
+++ b/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 unitree_go.msg.dds_ import LowState_
+from go2py_messages.msg.dds_ import Go2pyLowCmd_
+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.lowstate_topic_name = "rt/lowstate"
+ self.lowcmd_topic_name = "rt/go2py/low_cmd"
+ 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_reader = DataReader(self.participant, self.lowstate_topic)
+ self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, Go2pyState_)
+ 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.lowstate_thread = Thread(target = self.lowstate_update)
+ self.setCommands = {'lowlevel':self.setCommandsLow,
+ '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 lowstate_update(self):
+ def state_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
- 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':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp}
+ accel = self.state.accel
+ gyro = self.state.gyro
+ quat = self.state.quat
+ temp = self.state.imu_temp
+ return {'accel':accel, 'gyro':gyro, 'quat':quat, 'temp':temp}
def getFootContacts(self):
"""Returns the raw foot contact forces"""
- footContacts = self.state.foot_force
- return np.array(footContacts)
+ footContacts = self.state.contact
+ 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,
- 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]}
+ return {'q':self.state.q,
+ 'dq':self.state.dq,
+ 'tau_est':self.state.tau,
+ 'temperature':self.state.motor_temp}
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 batteryState.soc
+ return self.state.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)
+ highcmd = Go2pyHighCmd_(
+ _v_x,
+ _v_y,
+ _ω_z
+ )
+ 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
\ No newline at end of file
diff --git a/Go2Py/robot/interface/__init__.py b/Go2Py/robot/interface/__init__.py
deleted file mode 100644
index e69de29..0000000
diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py
deleted file mode 100644
index b0ecec1..0000000
--- a/Go2Py/robot/interface/ros2.py
+++ /dev/null
@@ -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()
\ No newline at end of file
diff --git a/Makefile b/Makefile
index 7d4b055..fa2b244 100644
--- a/Makefile
+++ b/Makefile
@@ -1,3 +1,6 @@
+messages:
+ @cd scripts && ./make_msgs.sh
+
realsense:
@cd deploy/docker && docker build --tag go2py_realsense:latest -f Dockerfile.realsense .
diff --git a/dds_messages/idls/BmsState_.idl b/dds_messages/idls/BmsState_.idl
deleted file mode 100644
index 9efe4c4..0000000
--- a/dds_messages/idls/BmsState_.idl
+++ /dev/null
@@ -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__
\ No newline at end of file
diff --git a/dds_messages/idls/IMUState_.idl b/dds_messages/idls/IMUState_.idl
deleted file mode 100644
index e1c2afe..0000000
--- a/dds_messages/idls/IMUState_.idl
+++ /dev/null
@@ -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__
\ No newline at end of file
diff --git a/dds_messages/idls/LowState_.idl b/dds_messages/idls/LowState_.idl
deleted file mode 100644
index 1b26cff..0000000
--- a/dds_messages/idls/LowState_.idl
+++ /dev/null
@@ -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__
\ No newline at end of file
diff --git a/dds_messages/idls/MotorState_.idl b/dds_messages/idls/MotorState_.idl
deleted file mode 100644
index a9bbeea..0000000
--- a/dds_messages/idls/MotorState_.idl
+++ /dev/null
@@ -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__
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt b/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt
index bddc9d8..a8456db 100644
--- a/deploy/ros2_nodes/go2py_bridge/CMakeLists.txt
+++ b/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)
diff --git a/deploy/ros2_nodes/go2py_bridge/package.xml b/deploy/ros2_nodes/go2py_bridge/package.xml
index 892289f..fd9038b 100644
--- a/deploy/ros2_nodes/go2py_bridge/package.xml
+++ b/deploy/ros2_nodes/go2py_bridge/package.xml
@@ -10,7 +10,7 @@
ament_cmake
unitree_go
- go2py_common
+ go2py_messages
unitree_api
rclcpp
std_msgs
diff --git a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp b/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
index 881fafc..943cd3d 100644
--- a/deploy/ros2_nodes/go2py_bridge/src/bridge.cpp
+++ b/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_common/msg/go2py_low_cmd.hpp"
+#include "go2py_messages/msg/go2py_status.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,112 +33,87 @@ 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("/go2/odom", 1);
pub_imu = this->create_publisher("/go2/imu", 1);
pub_joint = this->create_publisher("/go2/joint_states", 1);
- sub_twist = this->create_subscription("/go2/twist_cmd", 1, std::bind(&Custom::twistCmdCallback, this, std::placeholders::_1));
- nav2_twist = this->create_subscription("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
+ nav2_twist_subr = this->create_subscription("/go2/cmd_vel", 1, std::bind(&Custom::nav2TwistCmdCallback, this, std::placeholders::_1));
- // Go2 highlevel subscriber and publishers
- // the state_suber is set to subscribe "sportmodestate" topic
- highstate_suber = this->create_subscription(
- "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("/api/sport/request", 1);
-
- //Go2 lowlevel interface
+ //Unitree Topics
init_lowcmd();
- lowstate_suber = this->create_subscription(
- "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
-
- lowcmd_suber = this->create_subscription(
- "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
+ unitree_lowstate_suber = this->create_subscription(
+ "lowstate", 1, std::bind(&Custom::unitree_lowstate_callback, this, std::placeholders::_1));
+ // the req_puber is set to subscribe "/api/sport/request" topic with dt
+ unitree_highreq_puber = this->create_publisher("/api/sport/request", 1);
lowcmd_puber = this->create_publisher("/lowcmd", 1);
+
+ // Go2py topics
+ go2py_low_cmd_suber = this->create_subscription(
+ "/go2py/low_cmd", 1, std::bind(&Custom::go2py_low_cmd_callback, this, std::placeholders::_1));
+ go2py_high_cmd_suber = this->create_subscription(
+ "/go2py/high_cmd", 1, std::bind(&Custom::go2py_high_cmd_callback, this, std::placeholders::_1));
+ go2py_state_puber = this->create_publisher("/go2py/state", 1);
+ status_publisher = this->create_publisher("/go2py/status", 1);
api_publisher = this->create_publisher("/api/robot_state/request", 1);
- status_publisher = this->create_publisher("/go2py/status", 1);
-
}
private:
rclcpp::TimerBase::SharedPtr watchdog_timer;
void watchdog();
- // Highlevel twist command through standard ROS2 message type
- void twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
- void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+ void init_lowcmd();
+ int Estop = 0;
+ int sport_mode = 1;
+ xRockerBtnDataStruct _keyData;
+
+ // Standard ROS2
geometry_msgs::msg::TwistStamped twist_cmd;
- geometry_msgs::msg::Twist twist;
- rclcpp::Subscription::SharedPtr sub_twist;
- rclcpp::Subscription::SharedPtr nav2_twist;
- uint64_t last_highcmd_stamp = 0;
-
- // ROS2 standard joint state, IMU, and odometry publishers
+ void nav2TwistCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+ rclcpp::Subscription::SharedPtr nav2_twist_subr;
rclcpp::Publisher::SharedPtr pub_imu;
rclcpp::Publisher::SharedPtr pub_joint;
rclcpp::Publisher::SharedPtr pub_odom;
- // Highlevel interface
- void highstate_callback(unitree_go::msg::SportModeState::SharedPtr data);
- rclcpp::Subscription::SharedPtr highstate_suber;
- rclcpp::Publisher::SharedPtr highreq_puber;
- unitree_api::msg::Request highreq; // Unitree Go2 ROS2 request message
- SportClient sport_req;
-
- // Lowlevel interface
- void lowstate_callback(unitree_go::msg::LowState::SharedPtr data);
- void lowcmd_callback(go2py_common::msg::Go2pyLowCmd::SharedPtr data);
- rclcpp::Subscription::SharedPtr lowstate_suber;
- rclcpp::Subscription::SharedPtr lowcmd_suber;
- // A struct to store the highlevel states for later use
- rclcpp::Publisher::SharedPtr lowcmd_puber;
+ // Unitree Interface
+ unitree_api::msg::Request highreq;
unitree_go::msg::LowCmd lowcmd_msg;
- uint64_t last_lowcmd_stamp = 0;
- xRockerBtnDataStruct _keyData;
+ SportClient sport_req;
+ void unitree_lowstate_callback(unitree_go::msg::LowState::SharedPtr data);
+ rclcpp::Subscription::SharedPtr unitree_lowstate_suber;
+ rclcpp::Publisher::SharedPtr unitree_highreq_puber;
+ rclcpp::Publisher::SharedPtr lowcmd_puber;
rclcpp::Publisher::SharedPtr api_publisher;
- rclcpp::Publisher::SharedPtr status_publisher;
- void init_lowcmd()
- {
+ // Lowlevel interface
+ void go2py_low_cmd_callback(go2py_messages::msg::Go2pyLowCmd::SharedPtr data);
+ void go2py_high_cmd_callback(go2py_messages::msg::Go2pyHighCmd::SharedPtr data);
+ rclcpp::Subscription::SharedPtr go2py_low_cmd_suber;
+ rclcpp::Subscription::SharedPtr go2py_high_cmd_suber;
+ rclcpp::Publisher::SharedPtr go2py_state_puber;
+ rclcpp::Publisher::SharedPtr status_publisher;
- for (int i = 0; i < 20; i++)
- {
- lowcmd_msg.motor_cmd[i].mode = 0x01; //Set toque mode, 0x00 is passive mode
- lowcmd_msg.motor_cmd[i].q = PosStopF;
- lowcmd_msg.motor_cmd[i].kp = 0;
- lowcmd_msg.motor_cmd[i].dq = VelStopF;
- lowcmd_msg.motor_cmd[i].kd = 0;
- lowcmd_msg.motor_cmd[i].tau = 0;
- }
- }
-
- struct {
- 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)
+void Custom::init_lowcmd()
+{
+
+ for (int i = 0; i < 20; i++)
+ {
+ lowcmd_msg.motor_cmd[i].mode = 0x01; //Set toque mode, 0x00 is passive mode
+ lowcmd_msg.motor_cmd[i].q = PosStopF;
+ lowcmd_msg.motor_cmd[i].kp = 0;
+ lowcmd_msg.motor_cmd[i].dq = VelStopF;
+ lowcmd_msg.motor_cmd[i].kd = 0;
+ lowcmd_msg.motor_cmd[i].tau = 0;
+ }
+}
+
+void Custom::unitree_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(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(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();
+ auto msg = std::make_shared();
msg->estop = Estop;
msg->sport_mode = sport_mode;
status_publisher->publish(*msg);
diff --git a/deploy/ros2_nodes/go2py_common/msg/AudioData.msg b/deploy/ros2_nodes/go2py_common/msg/AudioData.msg
deleted file mode 100755
index 6e889bc..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/AudioData.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint64 time_frame
-uint8[] data
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg b/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg
deleted file mode 100755
index bdbf9c3..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/BmsCmd.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint8 off
-uint8[3] reserve
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/BmsState.msg b/deploy/ros2_nodes/go2py_common/msg/BmsState.msg
deleted file mode 100755
index 052e4d5..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/BmsState.msg
+++ /dev/null
@@ -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
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Error.msg b/deploy/ros2_nodes/go2py_common/msg/Error.msg
deleted file mode 100755
index 4a5a631..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/Error.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-uint32 source
-uint32 state
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg b/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg
deleted file mode 100755
index 3c294f3..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/Go2FrontVideoData.msg
+++ /dev/null
@@ -1,4 +0,0 @@
-uint64 time_frame
-uint8[] video720p
-uint8[] video360p
-uint8[] video180p
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg b/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg
deleted file mode 100755
index 4e6f3b2..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/HeightMap.msg
+++ /dev/null
@@ -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
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/IMUState.msg b/deploy/ros2_nodes/go2py_common/msg/IMUState.msg
deleted file mode 100755
index e065d04..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/IMUState.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-float32[4] quaternion
-float32[3] gyroscope
-float32[3] accelerometer
-float32[3] rpy
-int8 temperature
diff --git a/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg b/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg
deleted file mode 100755
index c43bd20..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/InterfaceConfig.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-uint8 mode
-uint8 value
-uint8[2] reserve
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/LidarState.msg b/deploy/ros2_nodes/go2py_common/msg/LidarState.msg
deleted file mode 100755
index d8b4eb2..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/LidarState.msg
+++ /dev/null
@@ -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
diff --git a/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg b/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg
deleted file mode 100755
index 464a31d..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/LowCmd.msg
+++ /dev/null
@@ -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
-
diff --git a/deploy/ros2_nodes/go2py_common/msg/LowState.msg b/deploy/ros2_nodes/go2py_common/msg/LowState.msg
deleted file mode 100755
index ceaed32..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/LowState.msg
+++ /dev/null
@@ -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
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg b/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg
deleted file mode 100755
index 67cd1bf..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/MotorCmd.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-uint8 mode
-float32 q
-float32 dq
-float32 tau
-float32 kp
-float32 kd
-uint32[3] reserve
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/MotorState.msg b/deploy/ros2_nodes/go2py_common/msg/MotorState.msg
deleted file mode 100755
index 2e1a739..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/MotorState.msg
+++ /dev/null
@@ -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
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg b/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg
deleted file mode 100755
index 08b4945..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/PathPoint.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-float32 t_from_start
-float32 x
-float32 y
-float32 yaw
-float32 vx
-float32 vy
-float32 vyaw
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Req.msg b/deploy/ros2_nodes/go2py_common/msg/Req.msg
deleted file mode 100755
index af14a7d..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/Req.msg
+++ /dev/null
@@ -1,2 +0,0 @@
-string uuid
-string body
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Res.msg b/deploy/ros2_nodes/go2py_common/msg/Res.msg
deleted file mode 100755
index 8070258..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/Res.msg
+++ /dev/null
@@ -1,3 +0,0 @@
-string uuid
-uint8[] data
-string body
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg b/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg
deleted file mode 100755
index e5ccb46..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/SportModeCmd.msg
+++ /dev/null
@@ -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
-
-
diff --git a/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg b/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg
deleted file mode 100755
index 7a6c1e6..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/SportModeState.msg
+++ /dev/null
@@ -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
-
-
diff --git a/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg b/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg
deleted file mode 100755
index 188b3c1..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/TimeSpec.msg
+++ /dev/null
@@ -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
diff --git a/deploy/ros2_nodes/go2py_common/msg/UwbState.msg b/deploy/ros2_nodes/go2py_common/msg/UwbState.msg
deleted file mode 100755
index ed1da3a..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/UwbState.msg
+++ /dev/null
@@ -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
-
-
diff --git a/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg b/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg
deleted file mode 100755
index db8ec91..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/UwbSwitch.msg
+++ /dev/null
@@ -1 +0,0 @@
-uint8 enabled
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg b/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg
deleted file mode 100755
index 12965ff..0000000
--- a/deploy/ros2_nodes/go2py_common/msg/WirelessController.msg
+++ /dev/null
@@ -1,5 +0,0 @@
-float32 lx
-float32 ly
-float32 rx
-float32 ry
-uint16 keys
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/CMakeLists.txt b/deploy/ros2_nodes/go2py_messages/CMakeLists.txt
similarity index 70%
rename from deploy/ros2_nodes/go2py_common/CMakeLists.txt
rename to deploy/ros2_nodes/go2py_messages/CMakeLists.txt
index dd3ef68..5e2cb40 100755
--- a/deploy/ros2_nodes/go2py_common/CMakeLists.txt
+++ b/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
)
diff --git a/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg
new file mode 100644
index 0000000..361dcc0
--- /dev/null
+++ b/deploy/ros2_nodes/go2py_messages/msg/Go2pyHighCmd.msg
@@ -0,0 +1,3 @@
+float32 vx
+float32 vy
+float32 wz
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2pyLowCmd.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyLowCmd.msg
similarity index 100%
rename from deploy/ros2_nodes/go2py_common/msg/Go2pyLowCmd.msg
rename to deploy/ros2_nodes/go2py_messages/msg/Go2pyLowCmd.msg
diff --git a/deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyState.msg
new file mode 100644
index 0000000..45070fa
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/deploy/ros2_nodes/go2py_common/msg/Go2pyStatus.msg b/deploy/ros2_nodes/go2py_messages/msg/Go2pyStatus.msg
similarity index 100%
rename from deploy/ros2_nodes/go2py_common/msg/Go2pyStatus.msg
rename to deploy/ros2_nodes/go2py_messages/msg/Go2pyStatus.msg
diff --git a/deploy/ros2_nodes/go2py_common/package.xml b/deploy/ros2_nodes/go2py_messages/package.xml
similarity index 96%
rename from deploy/ros2_nodes/go2py_common/package.xml
rename to deploy/ros2_nodes/go2py_messages/package.xml
index 5053c64..fe77d57 100755
--- a/deploy/ros2_nodes/go2py_common/package.xml
+++ b/deploy/ros2_nodes/go2py_messages/package.xml
@@ -1,7 +1,7 @@
- go2py_common
+ go2py_messages
0.0.0
TODO: Package description
czk
diff --git a/examples/00-robot-interface-DDS.ipynb b/examples/00-robot-interface copy.ipynb
similarity index 79%
rename from examples/00-robot-interface-DDS.ipynb
rename to examples/00-robot-interface copy.ipynb
index d773b42..253b6ac 100644
--- a/examples/00-robot-interface-DDS.ipynb
+++ b/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": {
diff --git a/examples/01-robot-interface-ROS2.ipynb b/examples/01-robot-interface-ROS2.ipynb
deleted file mode 100644
index 423e064..0000000
--- a/examples/01-robot-interface-ROS2.ipynb
+++ /dev/null
@@ -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
-}
diff --git a/examples/01-ros2-tools.ipynb b/examples/01-ros2-tools.ipynb
new file mode 100644
index 0000000..253b6ac
--- /dev/null
+++ b/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
+}
diff --git a/go2py_common/.idlpy_manifest b/go2py_common/.idlpy_manifest
deleted file mode 100644
index 4ff480a..0000000
--- a/go2py_common/.idlpy_manifest
+++ /dev/null
@@ -1,3 +0,0 @@
-Go2pyLowCmd
-msg
-
diff --git a/go2py_common/msg/.idlpy_manifest b/go2py_common/msg/.idlpy_manifest
deleted file mode 100644
index 4ab2cc0..0000000
--- a/go2py_common/msg/.idlpy_manifest
+++ /dev/null
@@ -1,3 +0,0 @@
-Go2pyLowCmd
-dds_
-
diff --git a/go2py_common/msg/__init__.py b/go2py_common/msg/__init__.py
deleted file mode 100644
index 9eb7ef5..0000000
--- a/go2py_common/msg/__init__.py
+++ /dev/null
@@ -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_", ]
diff --git a/go2py_common/msg/dds_/.idlpy_manifest b/go2py_common/msg/dds_/.idlpy_manifest
deleted file mode 100644
index bae2f4a..0000000
--- a/go2py_common/msg/dds_/.idlpy_manifest
+++ /dev/null
@@ -1,3 +0,0 @@
-Go2pyLowCmd
-
-Go2pyLowCmd_
diff --git a/go2py_common/msg/dds_/__init__.py b/go2py_common/msg/dds_/__init__.py
deleted file mode 100644
index f4ecbc5..0000000
--- a/go2py_common/msg/dds_/__init__.py
+++ /dev/null
@@ -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_", ]
diff --git a/go2py_messages/.idlpy_manifest b/go2py_messages/.idlpy_manifest
new file mode 100644
index 0000000..d21fd04
--- /dev/null
+++ b/go2py_messages/.idlpy_manifest
@@ -0,0 +1,11 @@
+Go2pyHighCmd
+msg
+
+
+Go2pyLowCmd
+msg
+
+
+Go2pyState
+msg
+
diff --git a/go2py_common/__init__.py b/go2py_messages/__init__.py
similarity index 84%
rename from go2py_common/__init__.py
rename to go2py_messages/__init__.py
index 29bab3d..b1e8e5a 100644
--- a/go2py_common/__init__.py
+++ b/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
"""
diff --git a/go2py_messages/msg/.idlpy_manifest b/go2py_messages/msg/.idlpy_manifest
new file mode 100644
index 0000000..3c5861d
--- /dev/null
+++ b/go2py_messages/msg/.idlpy_manifest
@@ -0,0 +1,11 @@
+Go2pyHighCmd
+dds_
+
+
+Go2pyLowCmd
+dds_
+
+
+Go2pyState
+dds_
+
diff --git a/unitree_go/msg/__init__.py b/go2py_messages/msg/__init__.py
similarity index 82%
rename from unitree_go/msg/__init__.py
rename to go2py_messages/msg/__init__.py
index f8cae9b..fe3a99b 100644
--- a/unitree_go/msg/__init__.py
+++ b/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
"""
diff --git a/go2py_messages/msg/dds_/.idlpy_manifest b/go2py_messages/msg/dds_/.idlpy_manifest
new file mode 100644
index 0000000..362eb92
--- /dev/null
+++ b/go2py_messages/msg/dds_/.idlpy_manifest
@@ -0,0 +1,11 @@
+Go2pyHighCmd
+
+Go2pyHighCmd_
+
+Go2pyLowCmd
+
+Go2pyLowCmd_
+
+Go2pyState
+
+Go2pyState_
diff --git a/unitree_go/msg/dds_/_IMUState_.py b/go2py_messages/msg/dds_/_Go2pyHighCmd.py
similarity index 53%
rename from unitree_go/msg/dds_/_IMUState_.py
rename to go2py_messages/msg/dds_/_Go2pyHighCmd.py
index 473d9cb..ddab137 100644
--- a/unitree_go/msg/dds_/_IMUState_.py
+++ b/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_
- IDL file: IMUState_.idl
+ Module: go2py_messages.msg.dds_
+ 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_"):
- quaternion: types.array[types.float32, 4]
- gyroscope: types.array[types.float32, 3]
- accelerometer: types.array[types.float32, 3]
- rpy: types.array[types.float32, 3]
- temperature: types.uint8
+class Go2pyHighCmd_(idl.IdlStruct, typename="go2py_messages.msg.dds_.Go2pyHighCmd_"):
+ vx: types.float32
+ vy: types.float32
+ vz: types.float32
diff --git a/go2py_common/msg/dds_/_Go2pyLowCmd.py b/go2py_messages/msg/dds_/_Go2pyLowCmd.py
similarity index 82%
rename from go2py_common/msg/dds_/_Go2pyLowCmd.py
rename to go2py_messages/msg/dds_/_Go2pyLowCmd.py
index 0f37ccd..9e19dbc 100644
--- a/go2py_common/msg/dds_/_Go2pyLowCmd.py
+++ b/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]
diff --git a/go2py_messages/msg/dds_/_Go2pyState.py b/go2py_messages/msg/dds_/_Go2pyState.py
new file mode 100644
index 0000000..d562faa
--- /dev/null
+++ b/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
+
+
diff --git a/go2py_messages/msg/dds_/__init__.py b/go2py_messages/msg/dds_/__init__.py
new file mode 100644
index 0000000..b26fdeb
--- /dev/null
+++ b/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_", ]
diff --git a/scripts/dds_messages/Go2pyHighCmd.idl b/scripts/dds_messages/Go2pyHighCmd.idl
new file mode 100644
index 0000000..7da6f03
--- /dev/null
+++ b/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;
+ };
+ };
+ };
+};
\ No newline at end of file
diff --git a/dds_messages/idls/Go2pyLowCmd.idl b/scripts/dds_messages/Go2pyLowCmd.idl
similarity index 88%
rename from dds_messages/idls/Go2pyLowCmd.idl
rename to scripts/dds_messages/Go2pyLowCmd.idl
index 6c944fe..22707df 100644
--- a/dds_messages/idls/Go2pyLowCmd.idl
+++ b/scripts/dds_messages/Go2pyLowCmd.idl
@@ -1,4 +1,4 @@
-module go2py_common {
+module go2py_messages {
module msg {
module dds_{
@final struct Go2pyLowCmd_ {
diff --git a/scripts/dds_messages/Go2pyState.idl b/scripts/dds_messages/Go2pyState.idl
new file mode 100644
index 0000000..fb94c7c
--- /dev/null
+++ b/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;
+ };
+ };
+ };
+};
\ No newline at end of file
diff --git a/dds_messages/make_msgs.sh b/scripts/make_msgs.sh
similarity index 62%
rename from dds_messages/make_msgs.sh
rename to scripts/make_msgs.sh
index 070decf..f5607a8 100755
--- a/dds_messages/make_msgs.sh
+++ b/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 ../go2py_common
+# rm -r ../unitree_go
+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 go2py_common ../
\ No newline at end of file
+# mv unitree_go ../
+mv go2py_messages ../
\ No newline at end of file
diff --git a/unitree_go/.idlpy_manifest b/unitree_go/.idlpy_manifest
deleted file mode 100644
index e5dd99d..0000000
--- a/unitree_go/.idlpy_manifest
+++ /dev/null
@@ -1,15 +0,0 @@
-BmsState_
-msg
-
-
-IMUState_
-msg
-
-
-LowState_
-msg
-
-
-MotorState_
-msg
-
diff --git a/unitree_go/__init__.py b/unitree_go/__init__.py
deleted file mode 100644
index b553006..0000000
--- a/unitree_go/__init__.py
+++ /dev/null
@@ -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", ]
diff --git a/unitree_go/msg/.idlpy_manifest b/unitree_go/msg/.idlpy_manifest
deleted file mode 100644
index 5ca29a3..0000000
--- a/unitree_go/msg/.idlpy_manifest
+++ /dev/null
@@ -1,15 +0,0 @@
-BmsState_
-dds_
-
-
-IMUState_
-dds_
-
-
-LowState_
-dds_
-
-
-MotorState_
-dds_
-
diff --git a/unitree_go/msg/dds_/.idlpy_manifest b/unitree_go/msg/dds_/.idlpy_manifest
deleted file mode 100644
index bc9d94d..0000000
--- a/unitree_go/msg/dds_/.idlpy_manifest
+++ /dev/null
@@ -1,15 +0,0 @@
-BmsState_
-
-BmsState_
-
-IMUState_
-
-IMUState_
-
-LowState_
-
-LowState_
-
-MotorState_
-
-MotorState_
diff --git a/unitree_go/msg/dds_/_BmsState_.py b/unitree_go/msg/dds_/_BmsState_.py
deleted file mode 100644
index baba8cc..0000000
--- a/unitree_go/msg/dds_/_BmsState_.py
+++ /dev/null
@@ -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]
-
-
diff --git a/unitree_go/msg/dds_/_LowState_.py b/unitree_go/msg/dds_/_LowState_.py
deleted file mode 100644
index 27b1175..0000000
--- a/unitree_go/msg/dds_/_LowState_.py
+++ /dev/null
@@ -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
-
-
diff --git a/unitree_go/msg/dds_/_MotorState_.py b/unitree_go/msg/dds_/_MotorState_.py
deleted file mode 100644
index 2d28c7b..0000000
--- a/unitree_go/msg/dds_/_MotorState_.py
+++ /dev/null
@@ -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]
-
-
diff --git a/unitree_go/msg/dds_/__init__.py b/unitree_go/msg/dds_/__init__.py
deleted file mode 100644
index a360471..0000000
--- a/unitree_go/msg/dds_/__init__.py
+++ /dev/null
@@ -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_", ]