La policy como nodo ROS 2

# Contêiner ROS 2 Humble só para falar com o robô Unitree (o unitree_ros2 suporta Humble/Foxy, não Jazzy/Lyrical).
# Testado em 19/10/2026: ros:humble-ros-base (arm64) + unitree_ros2 668d1ec (02/07/2026).
FROM ros:humble-ros-base
RUN apt-get update && apt-get install -y --no-install-recommends \
      ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
      ros-humble-rosbag2-storage-mcap git python3-colcon-common-extensions \
    && rm -rf /var/lib/apt/lists/*
WORKDIR /unitree
RUN git clone -q https://github.com/unitreerobotics/unitree_ros2 && cd unitree_ros2 && git checkout -q 668d1ec
RUN . /opt/ros/humble/setup.sh && cd unitree_ros2/cyclonedds_ws && \
    colcon build --packages-select unitree_api unitree_go unitree_hg > /dev/null
# Mesmo domínio e mesma interface do simulador (unitree_mujoco: DOMAIN_ID=1, INTERFACE="lo")
ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp ROS_DOMAIN_ID=1 \
    CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="lo"/></Interfaces></General></Domain></CycloneDDS>'
COPY entrada.sh /entrada.sh
ENTRYPOINT ["/bin/bash", "/entrada.sh"]

F=${FTH:-$HOME/fth}; cd $F/operar
docker build -q -t fth-humble kit/ros2 > /dev/null && echo "imagem fth-humble pronta"
docker run --rm fth-humble bash -c 'echo "ROS 2 $ROS_DISTRO · RMW $RMW_IMPLEMENTATION · domínio $ROS_DOMAIN_ID"; ros2 pkg list | grep unitree'

F=${FTH:-$HOME/fth}; cd $F/operar && . $F/.venv/bin/activate
python3 kit/sim.py --s 20 --suspenso > /dev/null &
until python3 kit/monitor.py --s 0.5 > /dev/null; do sleep 0.5; done
docker run --rm --network host fth-humble ros2 topic list 2>/dev/null
docker run --rm --network host fth-humble timeout -s INT 8 ros2 topic hz /lowstate --window 1000 2>/dev/null | awk '/average rate/ {r=$3} END {printf "average rate ≈ %d Hz\n", int((r+5)/10)*10}'
wait

"""no_politica.py — o esqueleto de uma política como nó ROS 2 (Humble, dentro do contêiner fth-humble).

Assina /lowstate (unitree_hg/msg/LowState), roda a "política" a 50 Hz e publica /lowcmd (unitree_hg/msg/LowCmd).
A política daqui é a mais segura possível: amortecimento (kp = 0, kd = 2) em todos os motores. Troque o miolo
de passo() pela sua rede (ONNX, PyTorch) — o resto do nó fica igual.
Para o robô real faltam duas coisas que o simulador não cobra: o CRC do comando (os exemplos oficiais
calculam: motor_crc_hg.cpp no unitree_ros2, CRC().Crc no SDK Python; o simulador não confere) e o laço
de 2 ms do exemplo oficial entre a política e o motor.
"""
import math
import sys

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


class Politica(Node):
    def __init__(self, segundos):
        super().__init__("fth_politica")
        self.estado, self.recebidos, self.enviados, self.incl = None, 0, 0, 0.0
        self.create_subscription(LowState, "lowstate", self.chegou, 10)
        self.pub = self.create_publisher(LowCmd, "lowcmd", 10)
        self.create_timer(0.02, self.passo)  # 50 Hz
        self.fim = self.get_clock().now().nanoseconds + int(segundos * 1e9)

    def chegou(self, msg):
        self.estado, self.recebidos = msg, self.recebidos + 1
        w, x, y, z = msg.imu_state.quaternion
        self.incl = math.degrees(math.acos(max(-1.0, min(1.0, 1 - 2 * (x * x + y * y)))))

    def passo(self):
        if self.estado is None:
            return  # sem estado, sem comando
        cmd = LowCmd()
        cmd.mode_pr, cmd.mode_machine = 0, self.estado.mode_machine
        for i in range(29):
            m = cmd.motor_cmd[i]
            m.mode, m.q, m.dq, m.tau, m.kp, m.kd = 1, 0.0, 0.0, 0.0, 0.0, 2.0
        self.pub.publish(cmd)
        self.enviados += 1


def main():
    segundos = float(sys.argv[1]) if len(sys.argv) > 1 else 5.0
    rclpy.init()
    no = Politica(segundos)
    while rclpy.ok() and no.get_clock().now().nanoseconds < no.fim:
        rclpy.spin_once(no, timeout_sec=0.05)
    taxa = no.enviados / segundos
    print(f"nó fth_politica: {segundos:g} s · /lowstate recebido: {'sim' if no.recebidos else 'NÃO'}"
          f" · tronco em pé (inclinação < 20°): {'sim' if no.incl < 20 else 'não'}")
    print(f"/lowcmd publicado a ≈ {round(taxa):.0f} Hz (alvo 50 Hz) · comando: amortecimento")
    no.destroy_node()
    rclpy.shutdown()
    sys.exit(0 if no.recebidos and 45 <= taxa <= 55 else 1)


if __name__ == "__main__":
    main()

F=${FTH:-$HOME/fth}; cd $F/operar && . $F/.venv/bin/activate
python3 kit/sim.py --s 15 --suspenso > /dev/null &
until python3 kit/monitor.py --s 0.5 > /dev/null; do sleep 0.5; done
docker run --rm --network host -v $PWD/kit/ros2:/kit:ro fth-humble python3 /kit/no_politica.py 5 2>/dev/null; echo "saída: $?"
wait