localization

deploy/deploy_real/configs/g1.yaml

@@ -13,6 +13,38 @@ leg_joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
 joint2motor_idx: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11,
                   12, 13, 14, 15, 16, 17, 18, 19, 20, 21,
                   22, 23, 24, 25, 26, 27, 28]
+
+raw_joint_order: [
+    'left_hip_pitch_joint',
+    'left_hip_roll_joint',
+    'left_hip_yaw_joint',
+    'left_knee_joint',
+    'left_ankle_pitch_joint',
+    'left_ankle_roll_joint',
+    'right_hip_pitch_joint',
+    'right_hip_roll_joint',
+    'right_hip_yaw_joint',
+    'right_knee_joint',
+    'right_ankle_pitch_joint',
+    'right_ankle_roll_joint',
+    'waist_yaw_joint',
+    'waist_roll_joint',
+    'waist_pitch_joint',
+    'left_shoulder_pitch_joint',
+    'left_shoulder_roll_joint',
+    'left_shoulder_yaw_joint',
+    'left_elbow_joint',
+    'left_wrist_roll_joint',
+    'left_wrist_pitch_joint',
+    'left_wrist_yaw_joint',
+    'right_shoulder_pitch_joint',
+    'right_shoulder_roll_joint',
+    'right_shoulder_yaw_joint',
+    'right_elbow_joint',
+    'right_wrist_roll_joint',
+    'right_wrist_pitch_joint',
+    'right_wrist_yaw_joint'
+]
 # kps: [100, 100, 100, 150, 40, 40, 100, 100, 100, 150, 40, 40]
 # kds: [2, 2, 2, 4, 2, 2, 2, 2, 2, 4, 2, 2]
 # default_angles: [-0.1,  0.0,  0.0,  0.3, -0.2, 0.0, 

deploy/deploy_real/localization/pelvis_pub.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+from pathlib import Path
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from unitree_hg.msg import LowState as LowStateHG
+from tf2_ros.buffer import Buffer
+from tf2_ros.transform_listener import TransformListener
+from tf2_ros import TransformBroadcaster, TransformStamped
+
+import numpy as np
+import yaml
+
+
+class PelvistoTrack(Node):
+    def __init__(self):
+        super().__init__('pelvis_track_publisher')
+
+        self.tf_buffer = Buffer()
+        self.tf_listener = TransformListener(self.tf_buffer, self)
+        self.tf_broadcaster = TransformBroadcaster(self)
+
+        self.timer = self.create_timer(0.05, self.on_timer)
+        
+    def on_timer(self):
+        try:
+            t = TransformStamped()
+
+            # Read message content and assign it to
+            # corresponding tf variables
+
+            t_lidar_pelvis = self.tf_buffer.lookup_transform(
+                'mid360_link_IMU', 
+                # 'zed2_camera_center',
+                'pelvis', rclpy.time.Time()
+            )
+
+            t.header.stamp = self.get_clock().now().to_msg()
+            # t.header.frame_id = 'zed_camera_center'
+            t.header.frame_id = 'body'
+            t.child_frame_id = 'pelvis'
+
+            # Turtle only exists in 2D, thus we get x and y translation
+            # coordinates from the message and set the z coordinate to 0
+            t.transform.translation.x = t_lidar_pelvis.transform.translation.x
+            t.transform.translation.y = t_lidar_pelvis.transform.translation.y
+            t.transform.translation.z = t_lidar_pelvis.transform.translation.z
+
+            t.transform.rotation.x = t_lidar_pelvis.transform.rotation.x
+            t.transform.rotation.y = t_lidar_pelvis.transform.rotation.y
+            t.transform.rotation.z = t_lidar_pelvis.transform.rotation.z
+            t.transform.rotation.w = t_lidar_pelvis.transform.rotation.w
+
+            # Send the transformation
+            self.tf_broadcaster.sendTransform(t)
+        except Exception as ex:
+            print(f'Could not transform mid360_link_IMU to pelvis: {ex}')
+
+
+def main():
+    rclpy.init()
+    node = PelvistoTrack()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

deploy/deploy_real/localization/state_pub.launch.py

@@ -0,0 +1,27 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
+
+    urdf = '../../resources/robots/g1_description/g1_29dof_rev_1_0_lidar.urdf'
+    with open(urdf, 'r') as infp:
+        robot_desc = infp.read()
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value='false',
+            description='Use simulation (Gazebo) clock if true'),
+        Node(
+            package='robot_state_publisher',
+            executable='robot_state_publisher',
+            name='robot_state_publisher',
+            output='screen',
+            parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
+            arguments=[urdf]),
+    ])

deploy/deploy_real/localization/state_pub.py

@@ -0,0 +1,64 @@
+from math import sin, cos, pi
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile
+from geometry_msgs.msg import Quaternion
+from sensor_msgs.msg import JointState
+# from tf2_ros import TransformBroadcaster, TransformStamped
+from unitree_hg.msg import LowCmd as LowCmdHG, LowState as LowStateHG
+import yaml
+
+class StatePublisher(Node):
+
+    def __init__(self):
+        super().__init__('state_publisher')
+        qos_profile = QoSProfile(depth=10)
+
+        with open('./configs/g1.yaml', 'r') as fp:
+            self.joint_names = yaml.safe_load(fp)['raw_joint_order']
+
+        self.low_state = LowStateHG()
+        self.low_state_subscriber = self.create_subscription(LowStateHG,
+                    'lowstate', self.on_low_state, 10)
+        self.joint_pub = self.create_publisher(JointState,
+                                               'joint_states', qos_profile)
+        self.nodeName = self.get_name()
+        self.get_logger().info("{0} started".format(self.nodeName))
+        self.joint_state = JointState()
+
+    def on_low_state(self, msg: LowStateHG):
+        self.low_state = msg
+        joint_state = self.joint_state
+        now = self.get_clock().now()
+        joint_state.header.stamp = now.to_msg()
+        joint_state.name = self.joint_names
+        joint_state.position = [0.0 for _ in self.joint_names]
+        joint_state.velocity = [0.0 for _ in self.joint_names]
+        
+        n:int = min(len(self.joint_names), len(self.low_state.motor_state))
+        for i in range(n):
+            joint_state.position[i] = self.low_state.motor_state[i].q
+            joint_state.velocity[i] = self.low_state.motor_state[i].dq
+        self.joint_pub.publish(joint_state)
+    
+    def run(self):
+        loop_rate = self.create_rate(10)
+        try:
+            # rclpy.spin()
+            while rclpy.ok():
+                rclpy.spin_once(self)
+                loop_rate.sleep()
+        except KeyboardInterrupt:
+            pass
+
+
+
+def main():
+    rclpy.init()
+    node = StatePublisher()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()