# Example only: replace enp3s0 with the actual wired interface.
# Confirm the current Unitree documentation for the delivered configuration.

ip link show
sudo ip addr flush dev enp3s0
sudo ip addr add 192.168.123.99/24 dev enp3s0
sudo ip link set enp3s0 up

source /opt/ros/humble/setup.bash
source "$HOME/unitree_ros2/cyclonedds_ws/install/setup.bash"
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
  <NetworkInterface name="enp3s0" priority="default" multicast="default" />
</Interfaces></General></Domain></CycloneDDS>'

ros2 topic list

sudo apt update
sudo apt install -y \
  cmake \
  g++ \
  build-essential \
  libyaml-cpp-dev \
  libeigen3-dev \
  libboost-all-dev \
  libspdlog-dev \
  libfmt-dev

git clone https://github.com/unitreerobotics/unitree_sdk2.git
cd unitree_sdk2

git rev-parse HEAD

mkdir -p build
cd build
cmake .. -DCMAKE_INSTALL_PREFIX=/opt/unitree_robotics
make -j"$(nproc)"
sudo make install

# Install ROS 2 Humble separately using the official ROS documentation.
# Then install Unitree's listed ROS 2 dependencies.

sudo apt install -y \
  ros-humble-rmw-cyclonedds-cpp \
  ros-humble-rosidl-generator-dds-idl \
  libyaml-cpp-dev

git clone https://github.com/unitreerobotics/unitree_ros2.git
cd unitree_ros2/cyclonedds_ws

source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

ros2 interface list | grep unitree_hg
ros2 interface show unitree_hg/msg/LowState

# Example simulation separation pattern.
# Check the current repository because file names and defaults may change.

robot: "g1"
robot_scene: "scene.xml"
domain_id: 1
interface: "lo"
print_scene_information: 1

# Keep simulation isolated from the physical robot's DDS domain.
# G1 low-level messages use the unitree_hg IDL family.

source /opt/ros/humble/setup.bash
source "$HOME/unitree_ros2/cyclonedds_ws/install/setup.bash"
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

ros2 interface show unitree_hg/msg/LowState
ros2 topic list | grep -E 'lowstate|secondary_imu'
ros2 topic info /rt/lowstate --verbose
ros2 topic hz /rt/lowstate

# In the simulation environment, use loopback and a non-hardware DDS domain.
source "$HOME/unitree_ros2/setup_local.sh"
export ROS_DOMAIN_ID=1

mkdir -p "$HOME/g1_research/bags"
cd "$HOME/g1_research/bags"

ros2 bag record \
  /rt/lowstate \
  /rt/secondary_imu \
  --output g1_static_pose_sim_01

#!/usr/bin/env python3
"""Read-only Unitree G1 telemetry logger.

Verify the current unitree_hg LowState definition before use.
This node subscribes only; it does not publish robot commands.
"""

import csv
from pathlib import Path

import rclpy
from rclpy.node import Node
from unitree_hg.msg import LowState


class G1TelemetryLogger(Node):
    def __init__(self) -> None:
        super().__init__("g1_telemetry_logger")
        output = Path.home() / "g1_research" / "g1_lowstate.csv"
        output.parent.mkdir(parents=True, exist_ok=True)

        self._file = output.open("w", newline="", encoding="utf-8")
        self._writer = csv.writer(self._file)
        self._header_written = False

        self.create_subscription(
            LowState,
            "/rt/lowstate",
            self._on_state,
            10,
        )
        self.get_logger().info(f"Logging read-only state to {output}")

    def _on_state(self, msg: LowState) -> None:
        motor_positions = [float(motor.q) for motor in msg.motor_state]
        motor_velocities = [float(motor.dq) for motor in msg.motor_state]
        rpy = [float(value) for value in msg.imu_state.rpy]

        if not self._header_written:
            header = ["ros_time_ns", "roll", "pitch", "yaw"]
            header += [f"q_{index}" for index in range(len(motor_positions))]
            header += [f"dq_{index}" for index in range(len(motor_velocities))]
            self._writer.writerow(header)
            self._header_written = True

        timestamp = self.get_clock().now().nanoseconds
        self._writer.writerow([timestamp, *rpy, *motor_positions, *motor_velocities])
        self._file.flush()

    def destroy_node(self) -> bool:
        self._file.close()
        return super().destroy_node()


def main() -> None:
    rclpy.init()
    node = G1TelemetryLogger()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()