TG3/tg3_local_teleop/test_head_control.py

#!/usr/bin/env python3
from __future__ import annotations

import unittest

from head_control import (
    HeadPitchController,
    head_pitch_command_axis,
    left_z_pressed,
    right_stick_vertical,
)


def controller() -> HeadPitchController:
    return HeadPitchController(
        deadzone=0.1,
        max_speed_rad_s=0.2,
        max_accel_rad_s2=0.5,
        min_pitch_rad=-0.1745329252,
        max_pitch_rad=0.872664626,
        axis_sign=-1.0,
        max_dt_s=0.1,
    )


class RightStickParserTest(unittest.TestCase):
    def test_accepts_only_finite_two_axis_right_stick(self) -> None:
        self.assertEqual(
            right_stick_vertical({"joystick": {"right": [0.5, -0.2]}}),
            0.5,
        )
        self.assertIsNone(right_stick_vertical({"joystick": {"right": [0.5]}}))
        self.assertIsNone(
            right_stick_vertical({"joystick": {"right": [float("nan"), 0.0]}})
        )
        self.assertIsNone(right_stick_vertical({"joystick": {"right": [2.0, 0.0]}}))

    def test_left_z_strictly_gates_head_pitch_axis(self) -> None:
        released = {
            "button": {"left": [0, 0, 0]},
            "joystick": {"right": [0.8, 0.1]},
        }
        pressed = {
            "button": {"left": [0, 0, 1]},
            "joystick": {"right": [0.8, 0.1]},
        }
        self.assertFalse(left_z_pressed(released))
        self.assertEqual(head_pitch_command_axis(released), 0.0)
        self.assertTrue(left_z_pressed(pressed))
        self.assertEqual(head_pitch_command_axis(pressed), 0.8)
        self.assertIsNone(
            head_pitch_command_axis(
                {
                    "button": {"left": [0, 0, "pressed"]},
                    "joystick": {"right": [0.8, 0.1]},
                }
            )
        )


class HeadPitchControllerTest(unittest.TestCase):
    def test_new_session_requires_neutral_before_direct_motion(self) -> None:
        head = controller()
        head.new_session()
        self.assertIsNone(
            head.update(
                0.0,
                armed=True,
                input_healthy=True,
                feedback_healthy=True,
                external_busy=False,
                axis=1.0,
                feedback_position_rad=0.1,
            )
        )
        self.assertTrue(head.require_neutral)
        head.update(
            0.1,
            armed=True,
            input_healthy=True,
            feedback_healthy=True,
            external_busy=False,
            axis=0.0,
            feedback_position_rad=0.1,
        )
        self.assertFalse(head.require_neutral)
        target = head.update(
            0.2,
            armed=True,
            input_healthy=True,
            feedback_healthy=True,
            external_busy=False,
            axis=1.0,
            feedback_position_rad=0.1,
        )
        self.assertIsNotNone(target)
        self.assertLess(target, 0.1)  # stick up -> negative pitch -> view up

    def test_center_stops_and_resynchronizes_to_real_feedback(self) -> None:
        head = controller()
        head.new_session()
        head.update(
            0.0,
            armed=True,
            input_healthy=True,
            feedback_healthy=True,
            external_busy=False,
            axis=0.0,
            feedback_position_rad=0.2,
        )
        head.update(
            0.1,
            armed=True,
            input_healthy=True,
            feedback_healthy=True,
            external_busy=False,
            axis=-1.0,
            feedback_position_rad=0.2,
        )
        self.assertTrue(head.active)
        self.assertIsNone(
            head.update(
                0.2,
                armed=True,
                input_healthy=True,
                feedback_healthy=True,
                external_busy=False,
                axis=0.0,
                feedback_position_rad=0.25,
            )
        )
        self.assertFalse(head.active)
        self.assertEqual(head.target_rad, 0.25)
        self.assertEqual(head.velocity_rad_s, 0.0)

    def test_feedback_gap_or_external_command_fails_closed(self) -> None:
        for feedback_healthy, external_busy in ((False, False), (True, True)):
            head = controller()
            head.new_session()
            head.update(
                0.0,
                armed=True,
                input_healthy=True,
                feedback_healthy=True,
                external_busy=False,
                axis=0.0,
                feedback_position_rad=0.0,
            )
            self.assertIsNone(
                head.update(
                    0.1,
                    armed=True,
                    input_healthy=True,
                    feedback_healthy=feedback_healthy,
                    external_busy=external_busy,
                    axis=1.0,
                    feedback_position_rad=0.0,
                )
            )
            self.assertTrue(head.require_neutral)
            self.assertFalse(head.active)

    def test_target_is_clamped_to_software_working_range(self) -> None:
        head = controller()
        head.new_session()
        head.update(
            0.0,
            armed=True,
            input_healthy=True,
            feedback_healthy=True,
            external_busy=False,
            axis=0.0,
            feedback_position_rad=-0.17,
        )
        target = None
        for index in range(1, 200):
            target = head.update(
                index * 0.1,
                armed=True,
                input_healthy=True,
                feedback_healthy=True,
                external_busy=False,
                axis=1.0,
                feedback_position_rad=-0.17,
            )
        self.assertEqual(target, head.min_pitch_rad)


if __name__ == "__main__":
    unittest.main()