diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c971bf4..65422ea 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -37,12 +37,12 @@ jobs: cmake \ git rosdep init || true - rosdep update + rosdep update || true - name: Workspace layout run: | mkdir -p ws/src - cp -r camera_ros lucy_bringup lucy_ros2_control ws/src/ + cp -r camera_ros lucy_bringup lucy_ros2_control lucy_config_generator ws/src/ # Do not `rm -rf /var/lib/apt/lists/*` before this: rosdep/apt need indexes for packages.ros.org. # Skip keys: not in this CI tree (thais_urdf), external/third-party (audio_common), or hardware agents not needed to compile/test. @@ -61,7 +61,7 @@ jobs: run: | source /opt/ros/humble/setup.bash colcon build --symlink-install \ - --packages-select camera_ros lucy_bringup lucy_ros2_control \ + --packages-select camera_ros lucy_bringup lucy_ros2_control lucy_config_generator \ --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=ON - name: colcon test @@ -71,9 +71,9 @@ jobs: source /opt/ros/humble/setup.bash source install/setup.bash colcon test \ - --packages-select camera_ros lucy_bringup lucy_ros2_control \ + --packages-select camera_ros lucy_bringup lucy_ros2_control lucy_config_generator \ --event-handlers console_direct+ \ - --pytest-args "--tb=short -vv" + --pytest-args -vv --tb=short - name: colcon test-result working-directory: ws @@ -106,11 +106,11 @@ jobs: --cov-report=html:build/coverage_reports/html_lucy_bringup \ --cov-report=term-missing \ --cov-branch - # lucy_ros2_control — YAML validation tests only (no C++ coverage here) - python3 -m pytest src/lucy_ros2_control/test/ \ - --cov=src/lucy_ros2_control/test \ - --cov-report=xml:build/coverage_reports/lucy_ros2_control.xml \ - --cov-report=html:build/coverage_reports/html_lucy_ros2_control \ + # lucy_config_generator — golden template tests + python3 -m pytest src/lucy_config_generator/test/ \ + --cov=src/lucy_config_generator/lucy_config_generator \ + --cov-report=xml:build/coverage_reports/lucy_config_generator.xml \ + --cov-report=html:build/coverage_reports/html_lucy_config_generator \ --cov-report=term-missing \ --cov-branch @@ -119,7 +119,7 @@ jobs: if: success() with: token: ${{ secrets.CODECOV_TOKEN }} - files: ws/build/coverage_reports/camera_ros.xml,ws/build/coverage_reports/lucy_bringup.xml,ws/build/coverage_reports/lucy_ros2_control.xml + files: ws/build/coverage_reports/camera_ros.xml,ws/build/coverage_reports/lucy_bringup.xml,ws/build/coverage_reports/lucy_config_generator.xml flags: lucy_ros_packages name: lucy_ros_packages fail_ci_if_error: false diff --git a/README.md b/README.md index 5514696..990f0de 100644 --- a/README.md +++ b/README.md @@ -6,15 +6,17 @@ ROS 2 **Humble** repository for **Lucy** (Sentience Robotics): runtime bringup, | Package | One-line role | |---------|----------------| -| [**lucy_bringup**](lucy_bringup/) | Jetson-oriented **system launch**: micro-ROS agents, `rosbridge_server`, RealSense, `camera_ros`, delayed [`lucy_ros2_control`](lucy_ros2_control/) bringup. | +| [**lucy_bringup**](lucy_bringup/) | Jetson **system launch**: micro-ROS agents, **`web_ros_api`** (rosbridge + **`lucy_config_pipeline`**), RealSense, `camera_ros`, delayed [`lucy_ros2_control`](lucy_ros2_control/) bringup; **`lucy_*_development`** composes the web stack with **`thais_urdf`** RViz/Gazebo. | | [**lucy_ros2_control**](lucy_ros2_control/) | **Hardware** `ros2_control` plugin (`LucySystemHardware`), controller YAML, `control.launch.py` for the real robot stack (no RViz/rosbridge in that launch). | +| [**lucy_config_generator**](lucy_config_generator/) | **Config pipeline**: reads **`thais_urdf`** hardware YAML and emits RP2040 firmware C, `ros2_control` xacro, and `controllers.yaml` (see package README). | +| [**lucy_config_pipeline**](lucy_config_pipeline/) | **Config store + `ConfigurePipeline` action**: validate YAML, generate artifacts, build/flash RP2040 firmware via `picotool` [README](lucy_config_pipeline/README.md). | | [**camera_ros**](camera_ros/) | GStreamer-based **MJPEG** → `sensor_msgs/CompressedImage`; client-aware activation. | Package names match directories (`` in each `package.xml`). ## How this repo fits the platform -- **Robot model, RViz, Gazebo, and “combo” launches** (real or sim + rosbridge) live in the sibling repo **[thais_urdf](https://github.com/Sentience-Robotics/thais_urdf)** (or your fork), package name `thais_urdf`. `lucy_ros2_control` expects URDF/xacro and meshes from that tree when using default paths. +- **Robot model, RViz, and Gazebo** live in the sibling repo **[thais_urdf](https://github.com/Sentience-Robotics/thais_urdf)** (package name `thais_urdf`). **`lucy_bringup`** owns **rosbridge + hardware config** (**`web_ros_api.launch.py`**) and **`lucy.launch.py`** (composition via **`real`**, **`rviz`**, **`gazebo`**). `lucy_ros2_control` expects URDF/xacro and meshes from **`thais_urdf`** when using default paths. - **Web control panel** and teleop semantics are **not** in this repo; they consume the same topics/controllers documented in lucy_ws docs. ## Requirements @@ -23,6 +25,10 @@ Package names match directories (`` in each `package.xml`). - **ROS**: [ROS 2 Humble](https://docs.ros.org/en/humble/Installation.html). - **Per-package extras**: Jetson-typical USB video and audio stacks for bringup; RealSense SDK stack for `realsense2_camera`; serial devices for micro-ROS. See each package README and `lucy_bringup/REALSENSE.md`. +## Picotool and passwordless sudo + +The **`lucy_config_pipeline`** flash phase runs **`sudo picotool`**. Copy-paste **sudoers** setup (Ubuntu 22.04) lives in **[lucy_config_pipeline/README.md — Passwordless sudo for picotool](lucy_config_pipeline/README.md#passwordless-sudo-for-picotool)** (same repository; no `../` path). + ## Building (colcon workspace) Treat this repository as **`src/lucy_ros_packages`** (clone the contents into that folder) **or** clone in place so that packages are direct children of your workspace `src/`: @@ -30,7 +36,7 @@ Treat this repository as **`src/lucy_ros_packages`** (clone the contents into th ```text lucy_ws/ └── src/ - ├── lucy_ros_packages/ # this repo: lucy_bringup, lucy_ros2_control, camera_ros + ├── lucy_ros_packages/ # this repo: lucy_bringup, lucy_ros2_control, lucy_config_generator, lucy_config_pipeline, camera_ros └── thais_urdf/ # robot description + sim launches (separate repo) ``` @@ -40,7 +46,7 @@ Example build: source /opt/ros/humble/setup.bash cd lucy_ws colcon build --symlink-install \ - --packages-select lucy_bringup lucy_ros2_control camera_ros + --packages-select lucy_bringup lucy_ros2_control lucy_config_generator lucy_config_pipeline camera_ros source install/setup.bash ``` @@ -48,7 +54,7 @@ To include simulation/description from the other repo: ```bash colcon build --symlink-install \ - --packages-select lucy_bringup lucy_ros2_control camera_ros thais_urdf + --packages-select lucy_bringup lucy_ros2_control lucy_config_generator lucy_config_pipeline camera_ros thais_urdf ``` ## Quick start @@ -70,11 +76,11 @@ From your **workspace root** (e.g. `lucy_ws`), with packages under `src/lucy_ros ```bash source /opt/ros/humble/setup.bash colcon build --symlink-install \ - --packages-select camera_ros lucy_bringup lucy_ros2_control \ + --packages-select camera_ros lucy_bringup lucy_ros2_control lucy_config_generator \ --cmake-args -DBUILD_TESTING=ON source install/setup.bash -colcon test --packages-select camera_ros lucy_bringup lucy_ros2_control --event-handlers console_direct+ +colcon test --packages-select camera_ros lucy_bringup lucy_ros2_control lucy_config_generator --event-handlers console_direct+ colcon test-result --verbose ``` @@ -94,31 +100,28 @@ python3 -m pytest src/lucy_ros_packages/lucy_bringup/test/ \ --cov-report=xml:build/coverage_reports/lucy_bringup.xml \ --cov-report=html:build/coverage_html/lucy_bringup -python3 -m pytest src/lucy_ros_packages/lucy_ros2_control/test/ \ - --cov=src/lucy_ros_packages/lucy_ros2_control/test \ - --cov-report=term-missing \ - --cov-report=xml:build/coverage_reports/lucy_ros2_control.xml \ - --cov-report=html:build/coverage_html/lucy_ros2_control ``` -**Meaningful line coverage** is mostly from **`camera_ros/scripts`**; bringup tests only **byte-compile** launch files, and `lucy_ros2_control` tests cover **YAML + Python helpers**, not the C++ hardware plugin. +**Meaningful line coverage** is mostly from **`camera_ros/scripts`**; bringup tests only **byte-compile** launch files. ## CI -GitHub Actions (`.github/workflows/ci.yml`) runs **`rosdep`**, **`colcon build`**, **`colcon test`**, then **`pytest-cov`** over all Python tests, producing Cobertura XML under `ws/build/coverage_reports/` for **`camera_ros`**, **`lucy_bringup`**, and **`lucy_ros2_control`**. Reports are uploaded to [**Codecov**](https://codecov.io) when **`CODECOV_TOKEN`** is set on the repo, and HTML/XML are attached as workflow artifacts (`coverage-lucy_ros_packages`). See [`doc/DEVELOPER.md`](doc/DEVELOPER.md) §5. +GitHub Actions (`.github/workflows/ci.yml`) runs **`rosdep`**, **`colcon build`**, **`colcon test`**, then **`pytest-cov`** over Python tests, producing Cobertura XML under `ws/build/coverage_reports/` for **`camera_ros`**, **`lucy_bringup`**, and **`lucy_config_generator`**. Reports are uploaded to [**Codecov**](https://codecov.io) when **`CODECOV_TOKEN`** is set on the repo, and HTML/XML are attached as workflow artifacts (`coverage-lucy_ros_packages`). See [`docs/DEVELOPER.md`](docs/DEVELOPER.md) §5. ## Documentation map | Doc | Audience | |-----|----------| | This file | Anyone cloning **this** repository | -| [**doc/DEVELOPER.md**](doc/DEVELOPER.md) | **Contributors** — build, CI, package internals, extension checklist | +| [**docs/DEVELOPER.md**](docs/DEVELOPER.md) | **Contributors** — build, CI, package internals, extension checklist | | [lucy_bringup/README.md](lucy_bringup/README.md) | Operators and integrators (devices, tmux, launch args) | | [lucy_ros2_control/README.md](lucy_ros2_control/README.md) | Control stack quick start | -| [**doc/ROS2_CONTROL.md**](doc/ROS2_CONTROL.md) | **ros2_control** — general concepts + Lucy (`LucySystemHardware`, topics, launches) | +| [**docs/ROS2_CONTROL.md**](docs/ROS2_CONTROL.md) | **ros2_control** — general concepts + Lucy (`LucySystemHardware`, topics, launches) | +| [**lucy_config_generator/README.md**](lucy_config_generator/README.md) | Hardware YAML → firmware C, `ros2_control` xacro, controllers | +| [**lucy_config_pipeline/README.md**](lucy_config_pipeline/README.md) | Config services + pipeline action (build/flash); [passwordless sudo for picotool](lucy_config_pipeline/README.md#passwordless-sudo-for-picotool) | | [camera_ros/README.md](camera_ros/README.md) | Camera topics, parameters, troubleshooting | -If these repos live under **`lucy_ws`**, see **`lucy_ws/docs/developer_lucy_packages.md`** (index into each repo’s `doc/DEVELOPER.md`) and **`lucy_ws/docs/simulation_and_visualization.md`** (full-stack pipeline). +If these repos live under **`lucy_ws`**, see **`lucy_ws/docs/developer_lucy_packages.md`** (index into each repo’s `docs/DEVELOPER.md`) and **`lucy_ws/docs/simulation_and_visualization.md`** (full-stack pipeline). ## License diff --git a/camera_ros/CMakeLists.txt b/camera_ros/CMakeLists.txt index 899d367..fe2ed0f 100644 --- a/camera_ros/CMakeLists.txt +++ b/camera_ros/CMakeLists.txt @@ -13,15 +13,6 @@ find_package(sensor_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(std_msgs REQUIRED) find_package(cv_bridge REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -# Generate Python classes for the custom service -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/GetInt.srv" - DEPENDENCIES std_msgs - ADD_LINTER_TESTS -) - # Bind-mounted / persisted build dirs can leave .../ament_cmake_python// as a directory # from an interrupted build; the next build then fails creating the symlink ("Is a directory"). if(TARGET ament_cmake_python_symlink_${PROJECT_NAME}) @@ -94,7 +85,4 @@ if(BUILD_TESTING) ) endif() -# Export the generated interfaces -ament_export_dependencies(rosidl_default_runtime) - ament_package() diff --git a/camera_ros/package.xml b/camera_ros/package.xml index ce2e256..9c19270 100644 --- a/camera_ros/package.xml +++ b/camera_ros/package.xml @@ -19,9 +19,6 @@ std_srvs std_msgs cv_bridge - rosidl_default_generators - rosidl_default_runtime - python3-opencv gstreamer1.0-tools gstreamer1.0-plugins-good @@ -33,8 +30,6 @@ python3-pytest-mock python3-pytest-cov - rosidl_interface_packages - ament_cmake diff --git a/camera_ros/scripts/camera_publisher.py b/camera_ros/scripts/camera_publisher.py index 335a0b4..cb973d1 100644 --- a/camera_ros/scripts/camera_publisher.py +++ b/camera_ros/scripts/camera_publisher.py @@ -19,7 +19,6 @@ from sensor_msgs.msg import CompressedImage from std_msgs.msg import Int32 from std_srvs.srv import SetBool -from camera_ros.srv import GetInt import cv2 FPS = 10.0 @@ -97,8 +96,6 @@ def __init__(self): # Create services for streaming control self.create_service(SetBool, 'start_streaming', self.start_streaming_callback) self.create_service(SetBool, 'stop_streaming', self.stop_streaming_callback) - self.create_service(GetInt, 'get_client_count', self.get_client_count) - # Subscribe to client count for automatic control self.create_subscription(Int32, '/client_count', self.client_count_callback, 10) @@ -107,10 +104,6 @@ def __init__(self): f"at {self.target_fps} FPS" ) - def get_client_count(self, request, response): - response.value = self.client_count - return response - def start_streaming_callback(self, request, response): """Service: Start streaming.""" if not self.is_streaming: diff --git a/camera_ros/test/test_camera_publisher.py b/camera_ros/test/test_camera_publisher.py index d6812c9..8297c90 100644 --- a/camera_ros/test/test_camera_publisher.py +++ b/camera_ros/test/test_camera_publisher.py @@ -258,33 +258,6 @@ def test_client_count_callback_stop( assert node.is_streaming is False assert node.client_count == 0 - @patch('cv2.VideoCapture') - @patch('subprocess.run') - def test_get_client_count_service( - self, mock_subprocess, mock_videocapture, rclpy_init_shutdown - ): - """Test get_client_count service.""" - # Import here to ensure path is set up - from camera_ros.srv import GetInt - from camera_publisher import CameraPublisher - - # Mock subprocess and VideoCapture - mock_subprocess.return_value.returncode = 0 - mock_subprocess.return_value.stdout = ( - "webcamproduct: usb-webcam:\n /dev/video6\n" - ) - mock_cap = MagicMock() - mock_cap.isOpened.return_value = True - mock_videocapture.return_value = mock_cap - - node = CameraPublisher() - node.client_count = 5 - - request = GetInt.Request() - response = node.get_client_count(request, GetInt.Response()) - - assert response.value == 5 - @patch('cv2.VideoCapture') @patch('subprocess.run') def test_topic_name_ext_camera( diff --git a/camera_ros/test/test_camera_stream_controller.py b/camera_ros/test/test_camera_stream_controller.py index d3d087e..d2be225 100644 --- a/camera_ros/test/test_camera_stream_controller.py +++ b/camera_ros/test/test_camera_stream_controller.py @@ -33,14 +33,21 @@ def rclpy_init_shutdown(): rclpy.shutdown() +@pytest.fixture +def controller_node(rclpy_init_shutdown): + """Create a CameraStreamController and destroy it before rclpy shuts down.""" + from camera_stream_controller import CameraStreamController + node = CameraStreamController() + yield node + node.destroy_node() + + class TestCameraStreamController: """Unit tests for CameraStreamController node.""" - def test_node_initialization(self, rclpy_init_shutdown): + def test_node_initialization(self, controller_node): """Test that controller node initializes correctly.""" - from camera_stream_controller import CameraStreamController - - node = CameraStreamController() + node = controller_node assert node.get_name() == 'camera_stream_controller' assert node.current_client_count == 0 @@ -48,11 +55,11 @@ def test_node_initialization(self, rclpy_init_shutdown): assert node.start_streaming_client is not None assert node.stop_streaming_client is not None - def test_client_count_callback_start(self, rclpy_init_shutdown): + def test_client_count_callback_start(self, controller_node): """Test client count callback starts streaming when clients > 0.""" from camera_stream_controller import CameraStreamController + node = controller_node with patch.object(CameraStreamController, 'start_streaming') as mock_start: - node = CameraStreamController() node.is_streaming = False msg = Int32() @@ -62,11 +69,11 @@ def test_client_count_callback_start(self, rclpy_init_shutdown): mock_start.assert_called_once() assert node.current_client_count == 1 - def test_client_count_callback_stop(self, rclpy_init_shutdown): + def test_client_count_callback_stop(self, controller_node): """Test client count callback stops streaming when clients == 0.""" from camera_stream_controller import CameraStreamController + node = controller_node with patch.object(CameraStreamController, 'stop_streaming') as mock_stop: - node = CameraStreamController() node.is_streaming = True msg = Int32() @@ -76,11 +83,9 @@ def test_client_count_callback_stop(self, rclpy_init_shutdown): mock_stop.assert_called_once() assert node.current_client_count == 0 - def test_start_streaming_service_available(self, rclpy_init_shutdown): + def test_start_streaming_service_available(self, controller_node): """Test start_streaming when service is available.""" - from camera_stream_controller import CameraStreamController - - node = CameraStreamController() + node = controller_node node.start_streaming_client.wait_for_service = Mock(return_value=True) node.start_streaming_client.call_async = Mock(return_value=MagicMock()) @@ -91,11 +96,9 @@ def test_start_streaming_service_available(self, rclpy_init_shutdown): ) node.start_streaming_client.call_async.assert_called_once() - def test_start_streaming_service_unavailable(self, rclpy_init_shutdown): + def test_start_streaming_service_unavailable(self, controller_node): """Test start_streaming when service is unavailable.""" - from camera_stream_controller import CameraStreamController - - node = CameraStreamController() + node = controller_node node.start_streaming_client.wait_for_service = Mock(return_value=False) node.get_logger = Mock() @@ -106,11 +109,9 @@ def test_start_streaming_service_unavailable(self, rclpy_init_shutdown): ) node.get_logger().warn.assert_called() - def test_stop_streaming_service_available(self, rclpy_init_shutdown): + def test_stop_streaming_service_available(self, controller_node): """Test stop_streaming when service is available.""" - from camera_stream_controller import CameraStreamController - - node = CameraStreamController() + node = controller_node node.stop_streaming_client.wait_for_service = Mock(return_value=True) node.stop_streaming_client.call_async = Mock(return_value=MagicMock()) @@ -121,11 +122,9 @@ def test_stop_streaming_service_available(self, rclpy_init_shutdown): ) node.stop_streaming_client.call_async.assert_called_once() - def test_stop_streaming_service_unavailable(self, rclpy_init_shutdown): + def test_stop_streaming_service_unavailable(self, controller_node): """Test stop_streaming when service is unavailable.""" - from camera_stream_controller import CameraStreamController - - node = CameraStreamController() + node = controller_node node.stop_streaming_client.wait_for_service = Mock(return_value=False) node.get_logger = Mock() diff --git a/doc/ROS2_CONTROL.md b/doc/ROS2_CONTROL.md deleted file mode 100644 index 9315e56..0000000 --- a/doc/ROS2_CONTROL.md +++ /dev/null @@ -1,125 +0,0 @@ -# ros2_control on Lucy (`lucy_ros_packages` + `thais_urdf`) - -ROS 2 **Humble**. This document explains **ros2_control**, and how it is implemented in Lucy: hardware plugin, topics, YAML, and launch files. URDF / xacro for `ros2_control` blocks live in **`thais_urdf`**; plugin binary and controller YAML live in **`lucy_ros2_control`**. - -**Related:** [`doc/DEVELOPER.md`](DEVELOPER.md) (repo layout, CI), [`../thais_urdf/doc/DEVELOPER.md`](../../thais_urdf/doc/DEVELOPER.md) (URDF, sim launches). - ---- - -## 1. What is ros2_control ? - -**ros2_control** is the layer between **high-level motion** (trajectories, teleop, UIs) and **hardware** (or sim). Typical components: - -| Component | Role | -|--------|------| -| **Controller manager** | Loads controllers, runs update loop at `update_rate`. | -| **Controllers** | Consume commands / goals and write **command interfaces** (e.g. position) on hardware handles. | -| **Broadcasters** | Read **state interfaces** and publish ROS messages (e.g. **`joint_state_broadcaster`** → `/joint_states`). | -| **Hardware interface (plugin)** | Maps command/state buffers to real devices or sim (`read()` / `write()` each cycle). | - -Clients (MoveIt, web panel, scripts) usually talk to **controllers** (e.g. `FollowJointTrajectory` on a `joint_trajectory_controller`), not directly to microcontrollers. The hardware plugin is responsible for whatever the real stack needs (CAN, EtherCAT, or—in Lucy’s case—ROS topics consumed by micro-ROS on RP2040 boards). - ---- - -## 2. Lucy: components and packages - -| Asset | Package | Notes | -|--------|---------|--------| -| **`LucySystemHardware`** | `lucy_ros2_control` | `hardware_interface::SystemInterface` plugin; publishes actuator commands for **real** hardware. | -| **`lucy_controllers.yaml`** | `lucy_ros2_control` | Declares `controller_manager`, broadcasters, `left_arm_controller` / `right_arm_controller` (`JointTrajectoryController`). | -| **`control.launch.py`** | `lucy_ros2_control` | `robot_state_publisher` + `ros2_control_node` + spawners. | -| **`` xacro** | `thais_urdf` | Two **system** blocks (left/right arms), plugin `LucySystemHardware` or `gz_ros2_control/GazeboSimSystem`, plus `publisher_topic` / `node_name` params. | - -Joint names in **`lucy_controllers.yaml`** must match the URDF / xacro **exactly**. Change YAML and **`thais_urdf`** together (see `doc/DEVELOPER.md` checklist). - ---- - -## 3. Data flow (Lucy, real robot) - -```mermaid -flowchart TB - C["Clients
Contorl Panel / External Interface"] - CM["controller_manager
2× trajectory controller
joint_state_broadcaster"] - WL["LucySystemHardware
left URDF block"] - WR["LucySystemHardware
right URDF block"] - AL["/actuators/left_arm"] - AR["/actuators/right_arm"] - ML["micro-ROS
left Pico"] - MR["micro-ROS
right Pico"] - JS["/joint_states"] - - C -->|"trajectory"| CM - CM --> WL - CM --> WR - CM --> JS - WL -->|"write JointState"| AL --> ML - WR -->|"write JointState"| AR --> MR -``` - -- **`/joint_states`**: fused state for **`robot_state_publisher`**, RViz, TF. Comes from **`joint_state_broadcaster`**, not from `/actuators/*`. -- **`/actuators/left_arm`** and **`/actuators/right_arm`**: command stream from **`LucySystemHardware::write()`**, **RELIABLE** QoS to match micro-ROS default subscribers. - ---- - -## 4. Hardware plugin behavior (`LucySystemHardware`) - -Configured per arm in **`thais_urdf`** → `inmoov/ros2_control/inmoov_ros2_control.xacro` (when `use_gazebo_sim:=false`): - -- **Left:** `publisher_topic` = `actuators/left_arm`, `node_name` = `lucy_hardware_interface_left_arm`. -- **Right:** `publisher_topic` = `actuators/right_arm`, `node_name` = `lucy_hardware_interface_right_arm`. - -Implementation notes (see `lucy_ros2_control/hardware/lucy_system.cpp`): - -- **`read()`**: no encoders; **`hw_positions_`** is set from **`hw_commands_`** (state follows last command). -- **`write()`**: builds **`sensor_msgs/msg/JointState`** with **header** and **position** array only. Firmware expects **nine** positions per arm in bus order; the URDF lists ten arm joints per side, so **`left_shoulder_y_link_joint`** / **`right_shoulder_y_link_joint`** are **omitted** from the published array (wiring/bus mapping). -- **QoS**: publisher is **RELIABLE**; **BEST_EFFORT** would not match typical micro-ROS subscriptions. - -Simulation (`use_gazebo_sim:=true`) uses **`gz_ros2_control/GazeboSimSystem`** instead—no `/actuators/*` traffic to Picos. - ---- - -## 5. Launch entry points (what runs ros2_control) - -| Launch | ros2_control | Typical extras | -|--------|--------------|----------------| -| **`ros2 launch lucy_bringup lucy.launch.py`** | Yes (after **3 s** delay via `control.launch.py`) | micro-ROS agents, rosbridge, cameras, RealSense | -| **`ros2 launch lucy_ros2_control control.launch.py`** | Yes | Minimal: `robot_state_publisher` only | -| **`ros2 launch thais_urdf rviz.launch.py`** | Yes | RViz, rosbridge | -| **`ros2 launch thais_urdf gazebo.launch.py`** | Yes (sim plugins) | Gazebo, RViz, rosbridge | - -`lucy.launch.py` starts **`lucy_ros2_control`** **after** micro-ROS agents and rosbridge so serial and the web socket are up before the control node spikes load. - -For **RViz alone** next to a running Jetson stack, see **`thais_urdf`** README: **`rviz_standalone.launch.py`**. - ---- - -## 6. Web control panel - -The panel sends trajectories to **`joint_trajectory_controller`** topics (defaults include **`/left_arm_controller/joint_trajectory`** and **`/right_arm_controller/joint_trajectory`**). That requires: - -1. **Controllers** spawned and **active** (not only `joint_state_broadcaster`). -2. **`ros2_control_node`** running with **`LucySystemHardware`** (real) or Gazebo system (sim). - -If managers or spawners failed, commands are published but **nothing updates command interfaces** → Picos see no new `/actuators/*` data. - ---- - -## 7. Operational pitfalls (integration) - -1. **Arm controllers inactive** — `write()` only runs when controllers drive those joints; verify **`left_arm_controller`** / **`right_arm_controller`** with `ros2 control list_controllers`. -2. **Sim URDF on hardware** — `LucySystemHardware` is absent when **`GazeboSimSystem`** is in the URDF; real Picos need `use_gazebo_sim:=false` in xacro. -3. **QoS** — keep actuator publishers **RELIABLE** for micro-ROS defaults. -4. **Cycle packaging** — `thais_urdf` does not `exec_depend` on `lucy_ros2_control`; install both packages in the same workspace/underlay so default `urdf_path` / controller YAML resolve. - ---- - -## 8. File index (quick) - -| Path | Purpose | -|------|---------| -| `lucy_ros2_control/hardware/*` | `LucySystemHardware` | -| `lucy_ros2_control/config/lucy_controllers.yaml` | Controllers + `extra_joints` for TF | -| `lucy_ros2_control/launch/control.launch.py` | Bringup snippet | -| `thais_urdf/inmoov/ros2_control/inmoov_ros2_control.xacro` | Plugins + joint list per arm | - -Maintainers: align changes with **`../../thais_urdf/doc/DEVELOPER.md`** and control-panel joint config when joint order or names change. diff --git a/docs/COLCON_IGNORE b/docs/COLCON_IGNORE new file mode 100644 index 0000000..00094af --- /dev/null +++ b/docs/COLCON_IGNORE @@ -0,0 +1 @@ +This directory holds Markdown only; colcon ignores it via this file. diff --git a/doc/DEVELOPER.md b/docs/DEVELOPER.md similarity index 71% rename from doc/DEVELOPER.md rename to docs/DEVELOPER.md index a864245..0b021fc 100644 --- a/doc/DEVELOPER.md +++ b/docs/DEVELOPER.md @@ -2,7 +2,7 @@ ROS 2 **Humble**. For **contributors** who change launch files, nodes, `ros2_control` config, or CI in **this repository only**. -**Sibling robot description / sim launches**: `../thais_urdf/doc/DEVELOPER.md` when both repos live under the same workspace `src/` (e.g. `lucy_ws/src`). On GitHub: [Sentience-Robotics/thais_urdf](https://github.com/Sentience-Robotics/thais_urdf) → `doc/DEVELOPER.md`. +**Sibling robot description / sim launches**: `../thais_urdf/docs/DEVELOPER.md` when both repos live under the same workspace `src/` (e.g. `lucy_ws/src`). On GitHub: [Sentience-Robotics/thais_urdf](https://github.com/Sentience-Robotics/thais_urdf) → `docs/DEVELOPER.md`. Conventions follow common packaging practice ([REP-149](https://www.ros.org/reps/rep-0149.html): accurate `package.xml`, install rules, documented launch entry points). @@ -14,6 +14,7 @@ Conventions follow common packaging practice ([REP-149](https://www.ros.org/reps |---------|----------------| | `lucy_bringup` | Jetson **system bringup**: micro-ROS agents, `rosbridge_server`, `camera_ros`, RealSense, delayed include of `lucy_ros2_control`. | | `lucy_ros2_control` | **Hardware** `ros2_control`: `LucySystemHardware` plugin, `lucy_controllers.yaml`, `control.launch.py`. | +| `lucy_config_generator` | Reads **`thais_urdf`** `config/hardware/active.yaml` (or selected export) → RP2040 `config_*.c`, `ros2_control` xacro, `controllers.yaml`. | | `camera_ros` | MJPEG → `sensor_msgs/msg/CompressedImage`; GStreamer pipeline; pytest. | --- @@ -22,11 +23,12 @@ Conventions follow common packaging practice ([REP-149](https://www.ros.org/reps ```text lucy_ros_packages/ -├── doc/ +├── docs/ │ ├── DEVELOPER.md # this file -│ └── ROS2_CONTROL.md # ros2_control concepts + Lucy implementation +│ └── ROS2_CONTROL.md # ros2_control concepts + Lucy implementation ├── lucy_bringup/ ├── lucy_ros2_control/ +├── lucy_config_generator/ └── camera_ros/ ``` @@ -38,7 +40,7 @@ lucy_ros_packages/ cd lucy_ws # or your colcon workspace root source /opt/ros/humble/setup.bash colcon build --symlink-install \ - --packages-select lucy_bringup lucy_ros2_control camera_ros + --packages-select lucy_bringup lucy_ros2_control lucy_config_generator camera_ros source install/setup.bash ``` @@ -73,8 +75,8 @@ source install/setup.bash |------|--------| | **Launch** | `ros2 launch lucy_ros2_control control.launch.py` | | **Plugin** | `lucy_ros2_control.xml` → `lucy_ros2_control/LucySystemHardware`; C++ implementation under `hardware/`. | -| **Config** | `config/lucy_controllers.yaml` — must match joint names in **thais_urdf** xacro (`inmoov_ros2_control.xacro`). | -| **Architecture** | **[doc/ROS2_CONTROL.md](ROS2_CONTROL.md)** — general ros2_control → Lucy topics, YAML, launches, pitfalls. | +| **Config** | `config/lucy_controllers.yaml` — must match joint names in **thais_urdf** xacro (`inmoov_ros2_control.xacro`) when you maintain controllers here; generated **`thais_urdf`** `controllers.yaml` must stay aligned when using **`lucy_config_generator`**. | +| **Architecture** | **[docs/ROS2_CONTROL.md](ROS2_CONTROL.md)** — general ros2_control → Lucy topics, YAML, launches, pitfalls. | **When you change controllers or joints** @@ -89,16 +91,23 @@ source install/setup.bash | **Launch** | `ros2 launch camera_ros camera.launch.py` (`fps`, `device`, USB ids, …). | | **Tests** | `colcon test --packages-select camera_ros` with `BUILD_TESTING=ON`. | +### `lucy_config_generator` + +| Item | Detail | +|------|--------| +| **CLI** | `ros2 run lucy_config_generator generate …` (see package **README**). | +| **Tests** | `colcon test --packages-select lucy_config_generator` — golden outputs for C, xacro, YAML. | + --- ## 5. CI `.github/workflows/ci.yml` runs in `osrf/ros:humble-desktop` on **pull_request** and on **push** to **`main` / `master` / `dev`** only (avoids duplicate runs when a PR branch is pushed): -- **`rosdep install --from-paths src`** for `camera_ros`, `lucy_bringup`, `lucy_ros2_control` +- **`rosdep install --from-paths src`** for `camera_ros`, `lucy_bringup`, `lucy_ros2_control`, `lucy_config_generator` - **`colcon build`** with `BUILD_TESTING=ON` -- **`colcon test`** — ament linters, `camera_ros` pytests, **`lucy_bringup`** launch `py_compile` tests, **`lucy_ros2_control`** YAML tests -- **`pytest-cov`** — `camera_ros` (`scripts/`), `lucy_bringup` (`launch/`), `lucy_ros2_control` (`test/`) → Cobertura XML + HTML under `ws/build/coverage_reports/`; **Codecov** flag `lucy_ros_packages` (optional **`CODECOV_TOKEN`**) +- **`colcon test`** — ament linters, `camera_ros` pytests, **`lucy_bringup`** launch `py_compile` tests, **`lucy_ros2_control`** YAML tests, **`lucy_config_generator`** golden tests +- **`pytest-cov`** — `camera_ros` (`scripts/`), `lucy_bringup` (`launch/`), `lucy_ros2_control` (`test/`), `lucy_config_generator` (`lucy_config_generator/`) → Cobertura XML + HTML under `ws/build/coverage_reports/`; **Codecov** flag `lucy_ros_packages` (optional **`CODECOV_TOKEN`**) Local commands: **README.md** → *Tests and coverage (local)*. @@ -109,7 +118,7 @@ Local commands: **README.md** → *Tests and coverage (local)*. 1. New **runtime** dependency → correct `package.xml` in the affected package. 2. New **launch or config** → `install()` in that package’s `CMakeLists.txt`. 3. New **topic/service** → document in the package `README.md`. -4. **Control / joints** → always coordinate with **`thais_urdf`** (see its `doc/DEVELOPER.md`). +4. **Control / joints** → always coordinate with **`thais_urdf`** (see its `docs/DEVELOPER.md`). --- @@ -120,5 +129,5 @@ Local commands: **README.md** → *Tests and coverage (local)*. | Full Jetson stack | `ros2 launch lucy_bringup lucy.launch.py` | | Control stack only | `ros2 launch lucy_ros2_control control.launch.py` (requires **`thais_urdf`** installed in overlay — provides default URDF share) | | USB camera | `ros2 launch camera_ros camera.launch.py` | - -RViz / Gazebo combo launches live in **`thais_urdf`**. \ No newline at end of file +| RViz / Gazebo + web panel | **`lucy_bringup`** **`lucy.launch.py`** with **`rviz`**, **`gazebo`**, **`real`** (see **`lucy_ws/README.md`**) | +| URDF + RViz/Gazebo without web | **`thais_urdf`** **`control.launch.py`** + **`rviz_standalone.launch.py`**, or **`gazebo.launch.py`** | \ No newline at end of file diff --git a/docs/ROS2_CONTROL.md b/docs/ROS2_CONTROL.md new file mode 100644 index 0000000..43201cf --- /dev/null +++ b/docs/ROS2_CONTROL.md @@ -0,0 +1,164 @@ +# ros2_control on Lucy (`lucy_ros_packages` + `thais_urdf`) + +ROS 2 **Humble**. This document explains **ros2_control**, and how it is implemented in Lucy: hardware plugin, topics, YAML, and launch files. URDF / xacro for `ros2_control` blocks live in **`thais_urdf`**; plugin binary and controller YAML live in **`lucy_ros2_control`**. + +**Related:** [`DEVELOPER.md`](DEVELOPER.md) (repo layout, CI), [`../thais_urdf/docs/DEVELOPER.md`](../../thais_urdf/docs/DEVELOPER.md) (URDF, sim launches). + +--- + +## 1. What is ros2_control ? + +**ros2_control** is the layer between **high-level motion** (trajectories, teleop, UIs) and **hardware** (or sim). Typical components: + +| Component | Role | +|--------|------| +| **Controller manager** | Loads controllers, runs update loop at `update_rate`. | +| **Controllers** | Consume commands / goals and write **command interfaces** (e.g. position) on hardware handles. | +| **Broadcasters** | Read **state interfaces** and publish ROS messages (e.g. **`joint_state_broadcaster`** → `/joint_states`). | +| **Hardware interface (plugin)** | Maps command/state buffers to real devices or sim (`read()` / `write()` each cycle). | + +Clients (MoveIt, web panel, scripts) usually talk to **controllers** (e.g. `FollowJointTrajectory` on a `joint_trajectory_controller`), not directly to microcontrollers. The hardware plugin is responsible for whatever the real stack needs (CAN, EtherCAT, or—in Lucy’s case—ROS topics consumed by micro-ROS on RP2040 boards). + +--- + +## 2. Lucy: components and packages + +| Asset | Package | Notes | +|--------|---------|--------| +| **`LucySystemHardware`** | `lucy_ros2_control` | `hardware_interface::SystemInterface` plugin; publishes actuator commands for **real** hardware. | +| **`lucy_controllers.yaml`** | `lucy_ros2_control` | Declares `controller_manager`, broadcasters, `left_arm_controller` / `right_arm_controller` (`JointTrajectoryController`). | +| **`control.launch.py`** | `lucy_ros2_control` | `robot_state_publisher` + `ros2_control_node` + spawners. | +| **`` xacro** | `thais_urdf` | Two **system** blocks (left/right arms), plugin `LucySystemHardware` or `gz_ros2_control/GazeboSimSystem`, plus `publisher_topic` / `node_name` params. | + +Joint names in **`lucy_controllers.yaml`** (or generated **`thais_urdf`** `controllers.yaml`) must match the URDF / xacro **exactly**. Change YAML and **`thais_urdf`** together (see `docs/DEVELOPER.md` checklist). + +--- + +## 3. Data flow (Lucy, real robot) + +```mermaid +flowchart TB + C["Clients
Contorl Panel / External Interface"] + CM["controller_manager
2× trajectory controller
joint_state_broadcaster"] + WL["LucySystemHardware
left URDF block"] + WR["LucySystemHardware
right URDF block"] + AL["/actuators/left_arm"] + AR["/actuators/right_arm"] + ML["micro-ROS
left Pico"] + MR["micro-ROS
right Pico"] + JS["/joint_states"] + + C -->|"trajectory"| CM + CM --> WL + CM --> WR + CM --> JS + WL -->|"write JointState"| AL --> ML + WR -->|"write JointState"| AR --> MR +``` + +- **`/joint_states`**: fused state for **`robot_state_publisher`**, RViz, TF. Comes from **`joint_state_broadcaster`**, not from `/actuators/*`. +- **`/actuators/left_arm`** and **`/actuators/right_arm`**: command stream from **`LucySystemHardware::write()`**, **RELIABLE** QoS to match micro-ROS default subscribers. + +--- + +## 4. Hardware plugin behavior (`LucySystemHardware`) + +`LucySystemHardware` is the only ros2_control plugin Lucy ships. The same plugin is selected for **real** and **mock/RViz** hardware; only Gazebo uses a different plugin (`gz_ros2_control/GazeboSimSystem`). + +| `use_gazebo_sim` | `use_mock_hardware` | Plugin selected | `publish_actuators` | +|------------------|---------------------|-----------------|---------------------| +| `false` | `false` | `lucy_ros2_control/LucySystemHardware` | `true` (default) | +| `false` | `true` | `lucy_ros2_control/LucySystemHardware` | `false` | +| `true` | *any* | `gz_ros2_control/GazeboSimSystem` | n/a | + +Per-arm parameters (xacro, real hardware path): + +- **Left:** `publisher_topic = actuators/left_arm`, `node_name = lucy_hardware_interface_left_arm`. +- **Right:** `publisher_topic = actuators/right_arm`, `node_name = lucy_hardware_interface_right_arm`. + +Implementation (`lucy_ros2_control/src/lucy_system.cpp`): + +- **`on_init()`** is a thin orchestrator over four steps, each a small member function: `validate_joints()` (interface checks), `configure_publisher()` (params + node/publisher), `init_joint_limits()` and `init_actuator_mappings()`. The side-effect-free parsing/validation/mapping core lives in `src/joint_config.{hpp,cpp}` (no live ROS node) and is unit-tested directly. `on_init` parses, per joint: + - The actuator mapping (`offset_deg`, `direction`, `scale`, `servo_min/max/default_deg`). + - URDF position envelope from `` (radians) into `joint_min_rad_` / `joint_max_rad_`. + - The `publish_actuators` boolean (defaults to `true`). +- **`read()`**: no encoders → `hw_positions_` mirrors the last clamped command. +- **`write()`** (in order): + 1. **URDF clamp** — every `hw_commands_[i]` is clamped to `[joint_min_rad_[i], joint_max_rad_[i]]`. `/joint_states` and the actuator publisher both reflect the clamped value, so out-of-range commands from MoveIt / LCP / CLI plateau at the URDF wall. + 2. **Mock short-circuit** — if `publish_actuators_` is `false`, return after the clamp (no micro-ROS traffic). RViz/mock therefore enforces URDF limits the same way as real hardware, without spamming `/actuators/*`. + 3. **Actuator mapping** — `joint_rad → joint_deg → servo_deg` via `(joint_deg / (direction*scale)) + offset_deg`, then clamped to `[servo_min_deg, servo_max_deg]` (`actuator_command_to_servo_rad()` in `joint_config`). + 4. **Publish** — `sensor_msgs/msg/JointState` (header + position only) at **RELIABLE** QoS to match micro-ROS defaults. Firmware expects **nine** positions per arm in bus order, so `left_shoulder_y_link_joint` / `right_shoulder_y_link_joint` are omitted from the arm publishers (those are torso joints). + +The same clamp helper (`src/include/position_limit_clamp.hpp`) is reused for the actuator-frame clamp. + +> **Gazebo caveat.** `gz_ros2_control` (upstream `humble`) does **not** apply the `` values inside `write()`. Gazebo may still respect joint limits coming from the spawned model/physics, but ros2_control-level URDF clamping in this repo is enforced by `LucySystemHardware` only. If consistent clamping in Gazebo becomes a hard requirement, the path is to either patch `gz_ros2_control` locally or wire `joint_limits_interface::PositionJointSaturationHandle` on the controllers. + +--- + +## 5. Launch entry points (what runs ros2_control) + +| Launch | ros2_control | Typical extras | +|--------|--------------|----------------| +| **`ros2 launch lucy_bringup lucy.launch.py`** | Yes (after **3 s** delay when not sim) | Always **`web_ros_api`**; **`real`** → micro-ROS + cameras; **`rviz`**; **`gazebo:=true` `real:=false`** → sim | +| **`ros2 launch lucy_ros2_control control.launch.py`** | Yes | Minimal: `robot_state_publisher` only | +| **`ros2 launch thais_urdf control.launch.py`** + **`rviz_standalone`** | Yes | Two processes: control stack, then RViz only (no rosbridge) | +| **`ros2 launch thais_urdf gazebo.launch.py`** | Yes (sim plugins) | Gazebo; optional RViz via **`start_rviz`** (no rosbridge) | + +`lucy.launch.py` starts **`web_ros_api`** first, then (when **`real:=true`**) micro-ROS agents, then after **3 s** includes **`thais_urdf`** **`control.launch.py`** when **`gazebo:=false`**, so serial and the web socket are up before the control node spikes load. + +For **RViz alone** next to a running Jetson stack, see **`thais_urdf`** README: **`rviz_standalone.launch.py`**. + +--- + +## 6. Web control panel + +The panel sends trajectories to **`joint_trajectory_controller`** topics (defaults include **`/left_arm_controller/joint_trajectory`** and **`/right_arm_controller/joint_trajectory`**). That requires: + +1. **Controllers** spawned and **active** (not only `joint_state_broadcaster`). +2. **`ros2_control_node`** running with **`LucySystemHardware`** (real) or Gazebo system (sim). + +If managers or spawners failed, commands are published but **nothing updates command interfaces** → Picos see no new `/actuators/*` data. + +--- + +## 7. Operational pitfalls (integration) + +1. **Arm controllers inactive** — `write()` only runs when controllers drive those joints; verify **`left_arm_controller`** / **`right_arm_controller`** with `ros2 control list_controllers`. +2. **Sim URDF on hardware** — `LucySystemHardware` is absent when **`GazeboSimSystem`** is in the URDF; real Picos need `use_gazebo_sim:=false` in xacro. +3. **QoS** — keep actuator publishers **RELIABLE** for micro-ROS defaults. +4. **Cycle packaging** — `thais_urdf` does not `exec_depend` on `lucy_ros2_control`; install both packages in the same workspace/underlay so default `urdf_path` / controller YAML resolve. +5. **URDF limits invisible at runtime** — `lucy_config_generator` must run before bringup so `` actually lands in the installed `inmoov_ros2_control.xacro`. Use the **GENERATE** step in the LCP activate workflow (or `ros2 service call /lucy_control/configure_pipeline …`) — see [§9](#9-config-pipeline-and-the-generate-step). +6. **Gazebo over-travel** — see the caveat at the end of §4. + +--- + +## 8. File index (quick) + +| Path | Purpose | +|------|---------| +| `lucy_ros2_control/src/lucy_system.cpp` | `LucySystemHardware` plugin (lifecycle, read/write) | +| `lucy_ros2_control/src/joint_config.{hpp,cpp}` | Pure on_init helpers: parsing, validation, actuator mapping, joint↔servo math | +| `lucy_ros2_control/src/include/position_limit_clamp.hpp` | Shared URDF / actuator-frame clamp | +| `lucy_ros2_control/test/test_position_limit_clamp.cpp` | gtest pinning the clamp algorithm | +| `lucy_ros2_control/test/test_joint_config.cpp` | gtest for the joint_config helpers | +| `lucy_ros2_control/config/lucy_controllers.yaml` | Controllers + `extra_joints` for TF | +| `lucy_ros2_control/launch/control.launch.py` | Bringup snippet | +| `lucy_config_generator/lucy_config_generator/templates/ros2_control.xacro.j2` | Source of `` and plugin selection | +| `lucy_config_generator/test/test_position_limits_unified.py` | URDF limits propagate to all `` blocks | +| `thais_urdf/description/ros2_control/inmoov_ros2_control.xacro` | Generated artefact: plugins + joint list per board | + +Maintainers: align changes with **`../../thais_urdf/docs/DEVELOPER.md`** and control-panel joint config when joint order or names change. + +--- + +## 9. Config pipeline and the GENERATE step + +`lucy_config_pipeline` exposes the `ConfigurePipeline` action. The internal phases are now: + +1. **VALIDATE** — schema-check `config/hardware/active.yaml`. +2. **GENERATE** — `lucy_config_generator` regenerates `inmoov_ros2_control.xacro` and `controllers.yaml` from the YAML and installs them into `thais_urdf/description/...` and `thais_urdf/config/...`. **Always runs**, including in `simulation_only` mode where BUILD/FLASH are skipped. +3. **BUILD** *(optional, skipped in `simulation_only`)* — compiles the RP2040 firmware C generated alongside the xacro. +4. **FLASH** *(optional, skipped in `simulation_only`)* — `sudo picotool` of the new firmware images. +5. **RELOAD** — calls `/lucy_control/restart` to re-read URDF + controller YAML; for Gazebo topology changes a relaunch is still required. + +Decoupling GENERATE from BUILD means the LCP "SIMULATION ONLY" toggle can update URDF limits, joint names, and controller wiring without touching firmware. Real-hardware runs include all five phases. diff --git a/lucy_bringup/CMakeLists.txt b/lucy_bringup/CMakeLists.txt index e86ae26..68e7dd2 100644 --- a/lucy_bringup/CMakeLists.txt +++ b/lucy_bringup/CMakeLists.txt @@ -22,6 +22,11 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(FILES + system_scripts/lucy_workspace.zsh.inc + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_pytest REQUIRED) diff --git a/lucy_bringup/README.md b/lucy_bringup/README.md index cfbcc79..22175f5 100644 --- a/lucy_bringup/README.md +++ b/lucy_bringup/README.md @@ -4,12 +4,18 @@ System launch files and scripts for the Lucy robot on NVIDIA Jetson AGX Orin. ## Overview -This package provides a unified way to launch all components of the Lucy robot system: -- Two micro-ROS agents (for RP2040 controllers on left and right arms) -- ROSBridge WebSocket server (for web interface communication) -- Audio capture and playback nodes (for stereo microphones and speakers) -- Intel RealSense D435i camera (for vision system with depth sensing) -- Web control panel interface +**`lucy.launch.py`** is the **single main entry**: it **always** includes **`web_ros_api.launch.py`** (rosbridge + **lucy_config_pipeline** for the control panel). Optional pieces are controlled by launch arguments (defaults match “full Jetson stack without RViz or Gazebo”): + +| Argument | Default | When true / meaning | +|----------|---------|---------------------| +| **`real`** | `true` | micro-ROS agents (serial arms), USB webcam (**`camera_ros`**), RealSense | +| **`rviz`** | `false` | RViz2 with **`robot_package`** RViz config (`use_sim_time:=false`). If **`gazebo:=true`**, forwarded as **`start_rviz`** to **`thais_urdf/gazebo.launch.py`** (no duplicate RViz). | +| **`gazebo`** | `false` | Include **`thais_urdf/gazebo.launch.py`**. **Requires `real:=false`** or launch aborts with **`RuntimeError`**. | +| **`robot_package`** | `thais_urdf` | **`control.launch.py`**, config paths, RViz config share | +| **`config_dir`** | *(empty)* | Override hardware YAML dir for **lucy_config_pipeline** | +| **`urdf_path`**, **`base_path`** | *(see launch file)* | Forwarded to **`thais_urdf`** Gazebo when **`gazebo:=true`** | + +Audio nodes are not wired in **`lucy.launch.py`** today (reserved for future use). ## Building @@ -23,11 +29,13 @@ source install/setup.bash ### Using the tmux Launcher (Recommended for Development) +Scripts resolve the colcon workspace as: **`LUCY_WS`** (if set and the directory exists), else from the script install path (`install/lib/lucy_bringup`) or source layout (`…/lucy_bringup/system_scripts`), else **`~/lucy_ws`**. The Vite app is started from **`${WORKSPACE}/src/lucy_control_panel`**. + ```bash # Launch everything ~/launch_lucy.sh -# Check system status +# Check system status (includes config pipeline + control panel probes) ~/check_lucy.sh # Stop everything @@ -40,13 +48,27 @@ source install/setup.bash # Source workspace source ~/lucy_ws/install/setup.zsh -# Launch only ROS nodes (no web interface) +# Default Jetson + panel (no RViz / no Gazebo) ros2 launch lucy_bringup lucy.launch.py -# Launch with custom devices +# Jetson + RViz + panel +ros2 launch lucy_bringup lucy.launch.py rviz:=true + +# Dev + panel + control + RViz (no micro-ROS / cameras) +ros2 launch lucy_bringup lucy.launch.py real:=false rviz:=true + +# Gazebo sim + panel (``rviz:=false`` = headless Gazebo) +ros2 launch lucy_bringup lucy.launch.py gazebo:=true real:=false + +# Custom serial devices or robot package ros2 launch lucy_bringup lucy.launch.py device0:=/dev/ttyACM2 device1:=/dev/ttyACM3 +ros2 launch lucy_bringup lucy.launch.py robot_package:=my_robot_urdf ``` +### Stopping (`stop_lucy.sh`) + +The script interrupts the ROS pane (twice), stops the web pane, kills the tmux session, then runs a **fallback cleanup** for common orphaned processes (`rosbridge_websocket*`, `micro_ros_agent`). It does **not** guarantee **every** node on the machine is gone: nodes started in another shell, another host on the same `ROS_DOMAIN_ID`, or names that linger briefly in discovery can still appear in `ros2 node list`. Use `ros2 node list` after stopping; if needed, stop other terminals or match remaining processes with `pgrep -af ros2`. + ## Architecture ### tmux Layout @@ -84,16 +106,9 @@ ros2 launch lucy_bringup lucy.launch.py device0:=/dev/ttyACM2 device1:=/dev/ttyA ## Launch Arguments -The `lucy.launch.py` file accepts the following arguments: - -- `device0` - Serial device for right arm (default: `/dev/ttyACM0`) -- `device1` - Serial device for left arm (default: `/dev/ttyACM1`) -- `realsense_serial` - RealSense camera serial number (default: `''` = auto-detect) +**`lucy.launch.py`:** `device0`, `device1`, `robot_package`, `config_dir`, **`real`**, **`rviz`**, **`gazebo`**, **`urdf_path`**, **`base_path`** (see **Overview** table). **`gazebo:=true`** with **`real:=true`** aborts at parse time. -Audio launch arguments (passed to `audio.launch.py`): -- `sample_rate` - Audio sample rate in Hz (default: `48000`) -- `capture_device` - Audio capture device index (default: `-1` for default) -- `playback_device` - Audio playback device index (default: `-1` for default) +**`realsense.launch.py`** is included as-is when **`real:=true`**; tune that file or wrap it if you need serial overrides. ## System Requirements @@ -154,12 +169,14 @@ These are informational warnings, not errors. The system continues to function n ``` lucy_bringup/ ├── launch/ -│ ├── lucy.launch.py # Main ROS2 launch file -│ └── realsense.launch.py # RealSense D435i camera launch file +│ ├── lucy.launch.py # Main entry (``real``, ``rviz``, ``gazebo``, …) +│ ├── web_ros_api.launch.py # rosbridge + lucy_config_pipeline only +│ └── realsense.launch.py # RealSense D435i camera launch file ├── system_scripts/ │ ├── launch_lucy.sh # tmux launcher │ ├── stop_lucy.sh # Graceful shutdown -│ └── check_lucy.sh # Health check +│ ├── check_lucy.sh # Health check +│ └── lucy_workspace.zsh.inc # Shared workspace path resolution (sourced by scripts) ├── CMakeLists.txt ├── package.xml ├── README.md # This file @@ -168,10 +185,15 @@ lucy_bringup/ ## Symlinks -For convenience, symlinks are created in `/home/dev/`: -- `~/launch_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/launch_lucy.sh` -- `~/stop_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/stop_lucy.sh` -- `~/check_lucy.sh` → `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/check_lucy.sh` +Point `~/` scripts at the **installed** copies so they sit next to `lucy_workspace.zsh.inc` (required for path resolution): + +```bash +ln -sf ~/lucy_ws/install/lib/lucy_bringup/launch_lucy.sh ~/launch_lucy.sh +ln -sf ~/lucy_ws/install/lib/lucy_bringup/stop_lucy.sh ~/stop_lucy.sh +ln -sf ~/lucy_ws/install/lib/lucy_bringup/check_lucy.sh ~/check_lucy.sh +``` + +You can also run them directly from `lucy_ws/src/lucy_ros_packages/lucy_bringup/system_scripts/` after `colcon build`. ## Camera System diff --git a/lucy_bringup/launch/lucy.launch.py b/lucy_bringup/launch/lucy.launch.py index f42b9e7..5f52bc9 100755 --- a/lucy_bringup/launch/lucy.launch.py +++ b/lucy_bringup/launch/lucy.launch.py @@ -15,218 +15,491 @@ # along with this program. If not, see . """ -Launch file for Lucy Robot System. - -This launch file starts all core ROS2 components: -- Two micro-ROS agents (for left and right arm RP2040 controllers) -- ros2_control (robot_state_publisher, controller_manager, - joint_state_broadcaster, arm controllers) -- ROSBridge WebSocket server (for web interface communication) -- Audio capture and playback nodes (for stereo microphones and speakers) -- RealSense D435i camera (for vision system with depth sensing) -- External USB webcam (for additional vision stream) - -Optimized for NVIDIA Jetson AGX Orin. +Single entry bringup for Lucy: web panel APIs, optional real peripherals, RViz, Gazebo. + +Always starts ``web_ros_api`` (rosbridge + lucy_config_pipeline). + +Invalid combinations raise at launch parse time (clear ``RuntimeError``). + +Arguments: +--------- +- ``real`` (default ``true``): micro-ROS agents, USB webcam, RealSense. When false, those + nodes are not constructed (``OpaqueFunction``), so e.g. Docker without ``micro_ros_agent`` + can run ``real:=false``. +- ``rviz`` (default ``false``): RViz2 (real/sim time set per mode). With ``gazebo:=true``, + forwarded as ``start_rviz`` to ``inmoov_urdf/gazebo.launch.py`` (no second RViz). +- ``gazebo`` (default ``false``): GZ Sim stack from ``inmoov_urdf``; requires ``real:=false``. +- ``headless`` (default ``false``): only meaningful with ``gazebo:=true``. Runs gz-sim + server-only with EGL rendering (``-s -r --headless-rendering``) so camera sensors + keep producing frames without an X server. Forwarded to ``inmoov_urdf/gazebo.launch.py``. + """ +import os +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import ( DeclareLaunchArgument, - ExecuteProcess, + GroupAction, IncludeLaunchDescription, LogInfo, + OpaqueFunction, TimerAction, ) +from launch.conditions import IfCondition, UnlessCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.substitutions import ( + Command, + LaunchConfiguration, + PathJoinSubstitution, + PythonExpression, +) +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue from launch_ros.substitutions import FindPackageShare -def create_micro_ros_nodes(device0, device1): - """Create micro-ROS agent nodes for left and right arms.""" - return [ - Node( - package='micro_ros_agent', - executable='micro_ros_agent', - name='micro_ros_agent_right', - arguments=['serial', '--dev', device0], - output='screen', - respawn=True, - respawn_delay=2.0, - emulate_tty=True - ), - Node( - package='micro_ros_agent', - executable='micro_ros_agent', - name='micro_ros_agent_left', - arguments=['serial', '--dev', device1], - output='screen', - respawn=True, - respawn_delay=2.0, - emulate_tty=True +def _infer_robot_source_root(robot_package: str, share_dir: str) -> Path: + """Prefer workspace src tree (pipeline writes) over install share.""" + cwd_candidate = Path.cwd() / "src" / robot_package + if cwd_candidate.is_dir(): + return cwd_candidate + share = Path(share_dir) + for parent in share.parents: + if parent.name == "install": + src_candidate = parent.parent / "src" / robot_package + if src_candidate.is_dir(): + return src_candidate + return share + + +def _discover_robot_packages(): + """ + List installed robot-description packages (launch/control.launch.py + description/). + + Identifies URDF/robot packages via their shared + layout, so the bringup can auto-select when only one is present. + """ + from ament_index_python.packages import get_packages_with_prefixes + + found = [] + for pkg, prefix in get_packages_with_prefixes().items(): + share = Path(prefix) / "share" / pkg + if (share / "launch" / "control.launch.py").is_file() and ( + share / "description" + ).is_dir(): + found.append(pkg) + return sorted(found) + + +def _default_robot_package(): + """ + Auto-pick the sole robot package; prefer inmoov_urdf when several are built. + + Returns an empty string when none is discovered, leaving ``robot_package`` + unset so ``_validate_lucy_launch`` can fail with a clear message instead of + silently assuming a package that is not installed. + """ + pkgs = _discover_robot_packages() + if len(pkgs) == 1: + return pkgs[0] + if "inmoov_urdf" in pkgs: + return "inmoov_urdf" + if pkgs: + return pkgs[0] + return "" + + +def _resolve_robot_paths(context): + """ + Fill urdf_path / base_path / controllers_yaml from the selected robot_package. + + Lets ``robot_package:=`` switch the URDF, base meshes and controllers + together. Explicit ``urdf_path`` / ``base_path`` / ``controllers_yaml`` + overrides (non-empty) are left untouched. + """ + from launch.actions import SetLaunchConfiguration + + robot_package = LaunchConfiguration("robot_package").perform(context).strip() + if not robot_package: + # _validate_lucy_launch already raised; nothing to resolve. + return [] + share = get_package_share_directory(robot_package) + robot_root = _infer_robot_source_root(robot_package, share) + + defaults = { + "urdf_path": str(robot_root / "description" / "urdf" / "inmoov.urdf.xacro"), + "base_path": str(robot_root / "description"), + "controllers_yaml": str(robot_root / "config" / "controllers.yaml"), + } + actions = [] + for key, default_value in defaults.items(): + if not LaunchConfiguration(key).perform(context).strip(): + actions.append(SetLaunchConfiguration(key, default_value)) + return actions + + +def _validate_lucy_launch(context): + gz = LaunchConfiguration("gazebo").perform(context).lower() + real = LaunchConfiguration("real").perform(context).lower() + robot_package = LaunchConfiguration("robot_package").perform(context).strip() + + if not robot_package: + raise RuntimeError( + "lucy.launch.py: no robot-description package found in the workspace " + "(expected one with launch/control.launch.py + description/, e.g. " + "inmoov_urdf). Clone/build a robot package, or pass one " + "explicitly with robot_package:=." ) - ] + + def _is_true(val): + return val in ("true", "1", "yes") + + if _is_true(gz) and _is_true(real): + raise RuntimeError( + "lucy.launch.py: gazebo:=true conflicts with real:=true. " + "Use real:=false for simulation (e.g. " + '"ros2 launch lucy_bringup lucy.launch.py gazebo:=true real:=false").' + ) + return [] -def create_audio_nodes(sample_rate, capture_device, playback_device): - """Create audio capture and playback nodes.""" +def create_micro_ros_nodes(device0: str, device1: str): + """Create micro-ROS agent nodes for left and right arms (device paths resolved).""" return [ Node( - package='audio_common', - executable='audio_capturer_node', - name='audio_capturer', - output='screen', + package="micro_ros_agent", + executable="micro_ros_agent", + name="micro_ros_agent_right", + arguments=["serial", "--dev", device0], + output="screen", respawn=True, respawn_delay=2.0, - parameters=[{ - 'format': 8, # paInt16 (PortAudio format constant) - 'channels': 2, # Stereo microphones - 'rate': sample_rate, - 'chunk': 1024, # Buffer size - 'device': capture_device, - 'frame_id': 'audio_capture' - }] + emulate_tty=True, ), Node( - package='audio_common', - executable='audio_player_node', - name='audio_player', - output='screen', + package="micro_ros_agent", + executable="micro_ros_agent", + name="micro_ros_agent_left", + arguments=["serial", "--dev", device1], + output="screen", respawn=True, respawn_delay=2.0, - parameters=[{ - 'channels': 2, # Stereo speakers - 'device': playback_device - }] - ) + emulate_tty=True, + ), ] +def _real_hardware_stack(context, *args, **kwargs): + """Build micro-ROS / camera / RealSense only when ``real`` is true (lazy package load).""" + real = LaunchConfiguration("real").perform(context).lower().strip() + if real not in ("true", "1", "yes"): + return [] + device0 = LaunchConfiguration("device0").perform(context) + device1 = LaunchConfiguration("device1").perform(context) + out = list(create_micro_ros_nodes(device0, device1)) + cam_share = get_package_share_directory("camera_ros") + out.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(cam_share, "launch", "camera.launch.py") + ), + ) + ) + lucy_share = get_package_share_directory("lucy_bringup") + out.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(lucy_share, "launch", "realsense.launch.py") + ), + ) + ) + return out + + def generate_launch_description(): """Generate launch description for Lucy robot system.""" - # Declare launch arguments for flexibility device0_arg = DeclareLaunchArgument( - 'device0', - default_value='/dev/ttyACM0', - description='Serial device for first micro-ROS agent (right arm)' + "device0", + default_value="/dev/ttyACM0", + description="Serial device for first micro-ROS agent (right arm)", ) device1_arg = DeclareLaunchArgument( - 'device1', - default_value='/dev/ttyACM1', - description='Serial device for second micro-ROS agent (left arm)' + "device1", + default_value="/dev/ttyACM1", + description="Serial device for second micro-ROS agent (left arm)", ) - # Audio launch arguments audio_sample_rate_arg = DeclareLaunchArgument( - 'audio_sample_rate', - default_value='48000', - description='Audio sample rate in Hz (e.g., 44100, 48000)' + "audio_sample_rate", + default_value="48000", + description="Audio sample rate in Hz (e.g., 44100, 48000)", ) audio_capture_device_arg = DeclareLaunchArgument( - 'audio_capture_device', - default_value='-1', - description='Audio capture device index (-1 for default)' + "audio_capture_device", + default_value="-1", + description="Audio capture device index (-1 for default)", ) audio_playback_device_arg = DeclareLaunchArgument( - 'audio_playback_device', - default_value='-1', - description='Audio playback device index (-1 for default)' - ) - - # Create subsystem nodes using helper functions - micro_ros_nodes = create_micro_ros_nodes( - LaunchConfiguration('device0'), - LaunchConfiguration('device1') - ) - - # Audio nodes are created but currently disabled - # audio_nodes = create_audio_nodes( - # LaunchConfiguration('audio_sample_rate'), - # LaunchConfiguration('audio_capture_device'), - # LaunchConfiguration('audio_playback_device') - # ) - - # ROSBridge WebSocket Server (for web interface) - rosbridge_server = ExecuteProcess( - cmd=['ros2', 'launch', 'rosbridge_server', 'rosbridge_websocket_launch.xml'], - output='screen', - shell=True - ) - - # External USB webcam (camera_ros package) - camera_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('camera_ros'), - 'launch', - 'camera.launch.py' - ]) - ]) - ) - - # RealSense D435i Camera - realsense_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('lucy_bringup'), - 'launch', - 'realsense.launch.py' - ]) - ]) - ) - - # ros2_control: robot_state_publisher, ros2_control_node, spawners - # (joint_state_broadcaster, left/right_arm_controller). - # Publishes /actuators/left_arm and /actuators/right_arm for micro-ROS. - # Started after a delay so micro_ros_agent and rosbridge are up first. - ros2_control_launch = TimerAction( - period=3.0, + "audio_playback_device", + default_value="-1", + description="Audio playback device index (-1 for default)", + ) + + robot_package_arg = DeclareLaunchArgument( + "robot_package", + default_value=_default_robot_package(), + description=( + "Robot package: control.launch.py + config paths + RViz config + URDF. " + "Defaults to the only installed robot package when just one is present, " + "else inmoov_urdf." + ), + ) + + config_dir_arg = DeclareLaunchArgument( + "config_dir", + default_value="", + description=( + "Override hardware config directory for lucy_config_pipeline " + "(empty = /config/hardware)" + ), + ) + + real_arg = DeclareLaunchArgument( + "real", + default_value="false", + description="If true: micro-ROS agents, USB webcam, RealSense", + ) + + rviz_arg = DeclareLaunchArgument( + "rviz", + default_value="false", + description="If true: RViz (or start_rviz when gazebo:=true)", + ) + + gazebo_arg = DeclareLaunchArgument( + "gazebo", + default_value="false", + description="If true: gazebo sim (requires real:=false)", + ) + + headless_arg = DeclareLaunchArgument( + "headless", + default_value="false", + description=( + "Only with gazebo:=true. Server-only gz-sim with EGL rendering " + "(-s -r --headless-rendering); cameras still work without an X server." + ), + ) + + # Empty defaults: _resolve_robot_paths fills these from the selected + # robot_package at launch time, so robot_package:= switches the URDF, + # base meshes and controllers together. Non-empty overrides are respected. + urdf_path_arg = DeclareLaunchArgument( + "urdf_path", + default_value="", + description=( + "URDF/xacro entry. Empty -> " + "/description/urdf/inmoov.urdf.xacro" + ), + ) + base_path_arg = DeclareLaunchArgument( + "base_path", + default_value="", + description="xacro base_path. Empty -> /description", + ) + urdf_path = LaunchConfiguration("urdf_path") + base_path = LaunchConfiguration("base_path") + controllers_yaml_arg = DeclareLaunchArgument( + "controllers_yaml", + default_value="", + description=( + "controllers.yaml path. Empty -> /config/controllers.yaml" + ), + ) + + validate = OpaqueFunction(function=_validate_lucy_launch) + resolve_robot_paths = OpaqueFunction(function=_resolve_robot_paths) + + web_ros_api_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare("lucy_bringup"), + "launch", + "web_ros_api.launch.py", + ] + ) + ] + ), + launch_arguments=[ + ("robot_package", LaunchConfiguration("robot_package")), + ("config_dir", LaunchConfiguration("config_dir")), + ], + ) + + controllers_yaml = LaunchConfiguration("controllers_yaml") + + # use_mock_hardware := (not gazebo) and (not real) — RViz-only mock_components path. + use_mock_hardware = PythonExpression( + [ + "'true' if ('", + LaunchConfiguration("gazebo"), + "'.lower() not in ('true','1','yes') and '", + LaunchConfiguration("real"), + "'.lower() not in ('true','1','yes')) else 'false'", + ] + ) + + # Force value_type=str so ROS 2 launch (Humble) does not try to YAML-parse + # the xacro output. The URDF starts with ``, which the YAML + # loader rejects with "Unable to parse the value of parameter robot_description". + robot_description = ParameterValue( + Command( + [ + "xacro ", + urdf_path, + " base_path:=", + base_path, + " use_gazebo_sim:=", + LaunchConfiguration("gazebo"), + " use_mock_hardware:=", + use_mock_hardware, + " controller_config:=", + controllers_yaml, + ] + ), + value_type=str, + ) + robot_description_dict = {"robot_description": robot_description} + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + robot_description_dict, + {"use_sim_time": LaunchConfiguration("gazebo")}, + ], + condition=IfCondition(LaunchConfiguration("gazebo")), + ) + + real_hardware = OpaqueFunction(function=_real_hardware_stack) + use_sim_time = LaunchConfiguration("gazebo") + + rviz = GroupAction( + condition=IfCondition(LaunchConfiguration("rviz")), actions=[ IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - PathJoinSubstitution([ - FindPackageShare('lucy_ros2_control'), - 'launch', - 'control.launch.py' - ]) - ]) - ) - ] + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare(LaunchConfiguration("robot_package")), + "launch", + "rviz.launch.py", + ] + ) + ] + ), + launch_arguments=[ + ("use_sim_time", use_sim_time), + ], + ), + ], + ) + + gazebo = GroupAction( + condition=IfCondition(LaunchConfiguration("gazebo")), + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare(LaunchConfiguration("robot_package")), + "launch", + "gazebo.launch.py", + ] + ) + ] + ), + launch_arguments=[ + ("urdf_path", LaunchConfiguration("urdf_path")), + ("base_path", LaunchConfiguration("base_path")), + ("controllers_yaml", controllers_yaml), + ("headless", LaunchConfiguration("headless")), + ], + ), + ], ) - return LaunchDescription([ - # Launch arguments - device0_arg, - device1_arg, - audio_sample_rate_arg, - audio_capture_device_arg, - audio_playback_device_arg, - - # Startup message - LogInfo(msg='========================================'), - LogInfo(msg='🤖 Starting Lucy Robot System...'), - LogInfo(msg='========================================'), - LogInfo(msg='Note: Audio underrun warnings are normal when no audio is published'), - - # Launch all subsystems - *micro_ros_nodes, # Unpack micro-ROS nodes - rosbridge_server, - # *audio_nodes, # Unpack audio nodes (currently disabled) - camera_launch, # External USB webcam - realsense_launch, # RealSense D435i camera - ros2_control_launch, # robot_state_publisher + ros2_control + spawners (after 3s delay) - - # Success message - LogInfo(msg='✅ All ROS nodes launched successfully!'), - LogInfo(msg=' - Micro-ROS Agents: right & left arm'), - LogInfo(msg=' - ros2_control: /actuators/left_arm, /actuators/right_arm for Picos'), - LogInfo(msg=' - ROSBridge Server: WebSocket ready'), - LogInfo(msg=' - Audio System: Capture & playback ready'), - LogInfo(msg=' - External USB Webcam: Stream ready'), - LogInfo(msg=' - RealSense D435i: Vision system active'), - LogInfo(msg='========================================'), - ]) + ros2_control_launch = GroupAction( + condition=UnlessCondition(LaunchConfiguration("gazebo")), + actions=[ + TimerAction( + period=3.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare( + LaunchConfiguration("robot_package") + ), + "launch", + "control.launch.py", + ] + ) + ] + ), + launch_arguments=[ + ("urdf_path", LaunchConfiguration("urdf_path")), + ("base_path", LaunchConfiguration("base_path")), + ("controllers_yaml", controllers_yaml), + ("use_mock_hardware", use_mock_hardware), + ], + ), + ], + ), + ], + ) + + return LaunchDescription( + [ + device0_arg, + device1_arg, + audio_sample_rate_arg, + audio_capture_device_arg, + audio_playback_device_arg, + robot_package_arg, + config_dir_arg, + real_arg, + rviz_arg, + gazebo_arg, + headless_arg, + urdf_path_arg, + base_path_arg, + controllers_yaml_arg, + validate, + resolve_robot_paths, + LogInfo(msg="========================================"), + LogInfo(msg="Starting lucy_bringup lucy.launch.py"), + LogInfo(msg="========================================"), + web_ros_api_launch, + real_hardware, + ros2_control_launch, + robot_state_publisher, + rviz, + gazebo, + LogInfo(msg="========================================"), + ] + ) diff --git a/lucy_bringup/launch/web_ros_api.launch.py b/lucy_bringup/launch/web_ros_api.launch.py new file mode 100644 index 0000000..9c775b1 --- /dev/null +++ b/lucy_bringup/launch/web_ros_api.launch.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +# Copyright 2025 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +""" +Web control panel ROS API: rosbridge WebSocket + lucy_config_pipeline. + +Exposes config services and rosbridge for the Vite panel. Composed by +``lucy.launch.py``. +""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + robot_package_arg = DeclareLaunchArgument( + 'robot_package', + default_value='thais_urdf', + description=( + 'Robot package share used for hardware YAML paths ' + 'in lucy_config_pipeline' + ), + ) + config_dir_arg = DeclareLaunchArgument( + 'config_dir', + default_value='', + description=( + 'Override hardware config directory for lucy_config_pipeline ' + '(empty = /config/hardware)' + ), + ) + + rosbridge = ExecuteProcess( + cmd=[ + 'ros2', + 'launch', + 'rosbridge_server', + 'rosbridge_websocket_launch.xml', + 'default_call_service_timeout:=5.0', + 'call_services_in_new_thread:=true', + 'send_action_goals_in_new_thread:=true', + ], + output='screen', + shell=True, + ) + + config_pipeline_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('lucy_config_pipeline'), + 'launch', + 'config_pipeline.launch.py', + ]) + ]), + launch_arguments=[ + ('robot_package', LaunchConfiguration('robot_package')), + ('config_dir', LaunchConfiguration('config_dir')), + ], + ) + + return LaunchDescription([ + robot_package_arg, + config_dir_arg, + rosbridge, + config_pipeline_launch, + ]) diff --git a/lucy_bringup/package.xml b/lucy_bringup/package.xml index 566a18b..7fe423d 100644 --- a/lucy_bringup/package.xml +++ b/lucy_bringup/package.xml @@ -3,7 +3,7 @@ lucy_bringup 1.0.0 - Launch files and scripts for bringing up the Lucy robot system + Launch files for Lucy bringup: lucy.launch.py (Jetson + optional RViz/Gazebo), web_ros_api (rosbridge + lucy_config_pipeline). Sentience Robotics Team GPL-3.0 @@ -16,6 +16,8 @@ camera_ros audio_common realsense2_camera + lucy_config_pipeline + thais_urdf launch launch_ros diff --git a/lucy_bringup/system_scripts/check_lucy.sh b/lucy_bringup/system_scripts/check_lucy.sh index 82deff1..ad80d0a 100755 --- a/lucy_bringup/system_scripts/check_lucy.sh +++ b/lucy_bringup/system_scripts/check_lucy.sh @@ -3,7 +3,10 @@ # Health check script for Lucy Robot System SESSION_NAME="lucy" -WORKSPACE="$HOME/lucy_ws" +_SCRIPT_DIR="${0:A:h}" +source "${_SCRIPT_DIR}/lucy_workspace.zsh.inc" +WORKSPACE="${LUCY_WORKSPACE}" +CONTROL_PANEL_DIR="${LUCY_CONTROL_PANEL_DIR}" # Colors for output GREEN='\033[0;32m' @@ -15,6 +18,8 @@ NC='\033[0m' # No Color echo -e "${BLUE}========================================${NC}" echo -e "${BLUE}🔍 Checking Lucy Robot System Status...${NC}" echo -e "${BLUE}========================================${NC}" +echo -e "${BLUE}Workspace:${NC} ${WORKSPACE}" +echo -e "${BLUE}Control panel:${NC} ${CONTROL_PANEL_DIR}" echo "" # Check if tmux session exists @@ -71,6 +76,28 @@ else echo -e "${RED}❌ Not found${NC}" fi +# Hardware config pipeline (from lucy_bringup lucy.launch.py) +echo -n " Config services (YAML): " +if ros2 service list 2>/dev/null | grep -Fq "/config/get"; then + echo -e "${GREEN}✅ config/get OK${NC}" +else + echo -e "${RED}❌ /config/get missing${NC}" +fi + +echo -n " Config pipeline node: " +if ros2 node list 2>/dev/null | grep -Fq "/lucy_config_pipeline"; then + echo -e "${GREEN}✅ lucy_config_pipeline${NC}" +else + echo -e "${YELLOW}⚠️ lucy_config_pipeline not in node list${NC}" +fi + +echo -n " ConfigurePipeline action: " +if ros2 action list 2>/dev/null | grep -Fq "/configure_pipeline"; then + echo -e "${GREEN}✅ /configure_pipeline${NC}" +else + echo -e "${YELLOW}⚠️ /configure_pipeline not listed${NC}" +fi + # Check realsense node echo -n " Realsense Camera: " if ros2 node list 2>/dev/null | grep -q "realsense2_camera"; then @@ -94,6 +121,36 @@ else echo -e "${RED}❌ Not found${NC}" fi +echo "" +echo -e "${BLUE}Control panel (Vite):${NC}" + +echo -n " Repository on disk: " +if [[ -d "${CONTROL_PANEL_DIR}" ]]; then + echo -e "${GREEN}✅ ${CONTROL_PANEL_DIR}${NC}" +else + echo -e "${RED}❌ Missing (expected under src/lucy_control_panel)${NC}" +fi + +# Match vite.config default (VITE_PORT or 5000) +_VITE_PORT="${VITE_PORT:-5000}" +echo -n " Dev server (port ${_VITE_PORT}): " +if command -v curl >/dev/null 2>&1; then + _code="$(curl -s -o /dev/null -w '%{http_code}' --connect-timeout 1 "http://127.0.0.1:${_VITE_PORT}/" 2>/dev/null)" || _code="" + [[ -z "${_code}" ]] && _code="000" + if [[ "${_code}" == 2* ]] || [[ "${_code}" == 3* ]]; then + echo -e "${GREEN}✅ HTTP ${_code} (pane 1 / yarn dev)${NC}" + else + echo -e "${YELLOW}⚠️ No response (HTTP ${_code}) — start via launch pane 1 if needed${NC}" + fi +else + if command -v ss >/dev/null 2>&1 && ss -tln 2>/dev/null | grep -q ":${_VITE_PORT}\\b"; then + echo -e "${GREEN}✅ Port ${_VITE_PORT} listening${NC}" + else + echo -e "${YELLOW}⚠️ Install curl or ss to probe; port ${_VITE_PORT} not detected listening${NC}" + fi +fi +unset _VITE_PORT _code + echo "" echo -e "${BLUE}Camera Services:${NC}" diff --git a/lucy_bringup/system_scripts/launch_lucy.sh b/lucy_bringup/system_scripts/launch_lucy.sh index 81d8f02..d99e421 100755 --- a/lucy_bringup/system_scripts/launch_lucy.sh +++ b/lucy_bringup/system_scripts/launch_lucy.sh @@ -5,8 +5,10 @@ set -e # Exit on error SESSION_NAME="lucy" -WORKSPACE="$HOME/lucy_ws" -WEB_DIR="$HOME/web_control_panel" +_SCRIPT_DIR="${0:A:h}" +source "${_SCRIPT_DIR}/lucy_workspace.zsh.inc" +WORKSPACE="${LUCY_WORKSPACE}" +WEB_DIR="${LUCY_CONTROL_PANEL_DIR}" # Colors for output GREEN='\033[0;32m' @@ -51,7 +53,7 @@ fi # Check if web directory exists if [ ! -d "$WEB_DIR" ]; then - echo -e "${YELLOW}⚠️ Web directory not found: $WEB_DIR${NC}" + echo -e "${YELLOW}⚠️ Control panel dir not found: $WEB_DIR${NC}" echo "Web interface will not be started." WEB_AVAILABLE=false else @@ -83,7 +85,7 @@ tmux resize-pane -t $SESSION_NAME:0.1 -x 20 # ============================================ echo -e "${BLUE}🤖 Setting up ROS2 nodes pane...${NC}" tmux send-keys -t $SESSION_NAME:0.0 "source $WORKSPACE/install/setup.zsh" C-m -tmux send-keys -t $SESSION_NAME:0.0 "ros2 launch lucy_bringup lucy.launch.py" C-m +tmux send-keys -t $SESSION_NAME:0.0 "ros2 launch lucy_bringup lucy.launch.py real:=true" C-m # ============================================ # PANE 1 (Bottom-Left): Web Interface diff --git a/lucy_bringup/system_scripts/lucy_workspace.zsh.inc b/lucy_bringup/system_scripts/lucy_workspace.zsh.inc new file mode 100644 index 0000000..50c98fd --- /dev/null +++ b/lucy_bringup/system_scripts/lucy_workspace.zsh.inc @@ -0,0 +1,23 @@ +# shellcheck shell=zsh +# Caller must set: _SCRIPT_DIR="${0:A:h}" immediately before sourcing this file. +# Exports LUCY_WORKSPACE and LUCY_CONTROL_PANEL_DIR. + +export LUCY_WORKSPACE="" +if [[ -n "${LUCY_WS}" && -d "${LUCY_WS}" ]]; then + export LUCY_WORKSPACE="${LUCY_WS:A}" +elif [[ "${_SCRIPT_DIR}" == */lib/lucy_bringup ]]; then + if _lw="$(builtin cd "${_SCRIPT_DIR}/../../.." 2>/dev/null && pwd)" && [[ -n "${_lw}" ]]; then + export LUCY_WORKSPACE="${_lw}" + fi + unset _lw +elif [[ "${_SCRIPT_DIR}" == */lucy_bringup/system_scripts ]]; then + if _lw="$(builtin cd "${_SCRIPT_DIR}/../../../.." 2>/dev/null && pwd)" && [[ -n "${_lw}" ]]; then + export LUCY_WORKSPACE="${_lw}" + fi + unset _lw +fi + +[[ -z "${LUCY_WORKSPACE}" ]] && export LUCY_WORKSPACE="${HOME}/lucy_ws" + +export LUCY_CONTROL_PANEL_DIR="${LUCY_WORKSPACE}/src/lucy_control_panel" +unset _SCRIPT_DIR diff --git a/lucy_bringup/system_scripts/stop_lucy.sh b/lucy_bringup/system_scripts/stop_lucy.sh index 45a25ac..f4b9ba6 100755 --- a/lucy_bringup/system_scripts/stop_lucy.sh +++ b/lucy_bringup/system_scripts/stop_lucy.sh @@ -3,6 +3,9 @@ # Stop script for Lucy Robot System SESSION_NAME="lucy" +_SCRIPT_DIR="${0:A:h}" +source "${_SCRIPT_DIR}/lucy_workspace.zsh.inc" +WORKSPACE="${LUCY_WORKSPACE}" # Colors for output GREEN='\033[0;32m' @@ -24,7 +27,7 @@ fi echo -e "${BLUE}📋 Gracefully stopping processes...${NC}" # Source ROS2 workspace for service calls -source $HOME/lucy_ws/install/setup.zsh 2>/dev/null || true +source "${WORKSPACE}/install/setup.zsh" 2>/dev/null || true # Stop camera streaming via service (with timeout) echo -e "${BLUE} → Stopping camera streaming...${NC}" @@ -35,9 +38,12 @@ else fi sleep 1 -# Stop ROS nodes (pane 0) +# Stop ROS nodes (pane 0): ros2 launch often needs a second SIGINT to tear down nested +# launches (e.g. rosbridge spawned via ExecuteProcess/shell). echo -e "${BLUE} → Stopping ROS2 nodes...${NC}" tmux send-keys -t $SESSION_NAME:0.0 C-c 2>/dev/null || true +sleep 3 +tmux send-keys -t $SESSION_NAME:0.0 C-c 2>/dev/null || true sleep 2 # Stop web interface (pane 1) @@ -45,9 +51,19 @@ echo -e "${BLUE} → Stopping web interface...${NC}" tmux send-keys -t $SESSION_NAME:0.1 C-c 2>/dev/null || true sleep 1 -# Kill the tmux session +# Kill the tmux session (sends SIGHUP to remaining pane processes) echo -e "${BLUE} → Terminating tmux session...${NC}" tmux kill-session -t $SESSION_NAME 2>/dev/null || true +sleep 1 + +# Fallback: processes launched under shell wrappers sometimes survive tmux teardown. +echo -e "${BLUE} → Cleaning up orphaned Lucy launch helpers (if any)...${NC}" +pkill -TERM -f rosbridge_websocket 2>/dev/null || true +pkill -TERM -f rosbridge_websocket_launch 2>/dev/null || true +pkill -TERM -f micro_ros_agent 2>/dev/null || true +sleep 2 +pkill -KILL -f rosbridge_websocket 2>/dev/null || true +pkill -KILL -f rosbridge_websocket_launch 2>/dev/null || true echo "" echo -e "${GREEN}========================================${NC}" diff --git a/lucy_cli/Readme.md b/lucy_cli/Readme.md new file mode 100644 index 0000000..00095a2 --- /dev/null +++ b/lucy_cli/Readme.md @@ -0,0 +1,36 @@ +# Lucy CLI + +This package provides a command-line interface for interacting with Lucy. + +## Installation + +1. **Clone the repository:** + ```bash + git clone https://github.com/Sentience-Robotics/lucy_ros_packages + ``` +2. **Build the package:** + Navigate to your ROS 2 workspace and build the package using `colcon`: + ```bash + colcon build --packages-select lucy_cli + ``` +3. **Source the workspace:** + ```bash + source install/setup.bash + ``` + +## Usage + +The main command provided by this package is `tui`, which launches a textual user interface. + +To run the TUI, use the following command: +```bash +ros2 run lucy_cli tui +``` + +## For Developers + +For more information on the development of this package, please see the [developer documentation](developer.md). + +## License + +This project is licensed under the GPL-3.0 License. See the `LICENSE` file for more details. diff --git a/lucy_cli/developer.md b/lucy_cli/developer.md new file mode 100644 index 0000000..b40a690 --- /dev/null +++ b/lucy_cli/developer.md @@ -0,0 +1,191 @@ +# Developer Guide: Creating a New Lucy Interface + +This guide explains how to build a new client application (e.g., a web UI, a different command-line tool, a Python script for automation) that can interact with and control a Lucy-enabled robot. + +The core of the Lucy control system is a set of ROS 2 topics and services. By interacting with these, any application can view robot state, get its hardware configuration, and send commands. + +The recommended way to interface with Lucy is to use the `lucy_cli.ros_interface.LucyROSInterface` class as a reference. It provides a high-level, well-documented Python API that handles all the underlying ROS 2 complexity. This guide will walk through the key concepts and provide code snippets based on that reference implementation. + +> Please note that the LucyROSInterface class is a simple example. You will find that more topics & services are available if needed, with for example a complete configuration workflow. If you need more feature or want to write your interface in another language, please check the differents packages available in this repository. The [Lucy Control Panel](https://github.com/Sentience-Robotics/lucy_control_panel/tree/master/) is a good example of a more in-depth implementation. + +## 1. Core Concepts + +### Control Arbitration (Taking and Releasing Control) + +To prevent multiple users from sending conflicting commands simultaneously, Lucy uses a simple control arbitration system. + +- **Topic:** `/control_panel_active_client` +- **Message Type:** `std_msgs/msg/String` +- **QoS:** Latching (`TRANSIENT_LOCAL` durability) + +**Protocol:** +1. **To Take Control:** Publish your unique client ID to this topic. +2. **To Release Control:** Publish an empty string (`""`) to this topic. +3. **To Monitor Control:** Subscribe to this topic. If you receive a message containing a client ID that is *not* your own, your application should enter a "read-only" mode and disable its own command publishing features. + +A good client ID is one that is both unique and descriptive, for example: `my_app_1678886400_a3f7`. + +```python +# From lucy_cli/ros_interface.py + +import time +import random + +# A unique identifier for this specific client instance. +CLIENT_ID = f"my_app_{int(time.time())}_{random.randint(1000, 9999)}" + +# ... inside your ROS Node class ... +# self.control_publisher = self.create_publisher(String, '/control_panel_active_client', qos_profile=LATCHING_QOS) + +def take_control(self): + """Publishes this client's ID to request control of the robot.""" + self.get_logger().info("Requesting control...") + msg = String(data=CLIENT_ID) + self.control_publisher.publish(msg) + +def release_control(self): + """Publishes an empty string to release control of the robot.""" + self.get_logger().info("Releasing control...") + msg = String(data="") + self.control_publisher.publish(msg) +``` + +### Dynamic Hardware Configuration + +You should **never** hardcode joint names, limits, or controller topics in your application. Lucy provides a service to fetch the currently active hardware configuration from the robot. + +- **Service:** `/config/get` +- **Service Type:** `lucy_msgs/srv/GetConfig` + +To get the currently active configuration, call this service with an empty `config_name`. The service will return a YAML string containing all the necessary information. + +```python +# From lucy_cli/ros_interface.py + +def get_hardware_config_yaml(self) -> str | None: + """ + Synchronously calls the /config/get service to fetch the active config. + """ + try: + # Wait for the service to be available + if not self._get_config_client.wait_for_service(timeout_sec=5.0): + self.get_logger().error("'/config/get' service not available.") + return None + + # Call the service with an empty name to get the active config + req = GetConfig.Request(config_name="") + future = self._get_config_client.call_async(req) + + # This requires spinning a temporary node to wait for the result + temp_node = Node('temp_service_caller') + rclpy.spin_until_future_complete(temp_node, future, timeout_sec=5.0) + temp_node.destroy_node() + + response = future.result() + if not response or not response.success: + self.get_logger().error(f"Failed to get active config: {response.message if response else 'timeout'}") + return None + + self.get_logger().info("Successfully fetched active hardware configuration.") + return response.config_yaml + except Exception as e: + self.get_logger().error(f"Exception in get_hardware_config_yaml: {e}") + return None +``` + +The returned YAML needs to be parsed to build your UI and command structures. See `tui_node.py`'s `parse_config_yaml` function for a reference implementation. + +## 2. Sending Commands + +### Commanding Joint Positions + +To move the robot's joints, you must publish to the appropriate [`ros2_control`](https://control.ros.org/rolling/index.html) controller topic. These topics are discovered from the hardware configuration YAML. + +- **Topic:** Dynamically determined (e.g., `/left_arm_controller/joint_trajectory`) +- **Message Type:** `trajectory_msgs/msg/JointTrajectory` +- **QoS:** Volatile (`VOLATILE` durability) + +A `JointTrajectory` message contains a list of joint names and one or more `JointTrajectoryPoint`s. For direct position control, you only need to send one point. + +**Important:** The `joint_names` list **must** be in the exact order expected by the controller, which is the order you get from parsing the hardware config YAML. + +```python +# From lucy_cli/ros_interface.py + +def publish_joint_trajectory(self, controller_topic: str, joint_names: list[str], positions_rad: list[float]): + """ + Publishes a JointTrajectory message to a specified controller. + """ + # Create a publisher for this topic if it doesn't exist yet + if controller_topic not in self._joint_publishers: + self.get_logger().warn(f"Creating new publisher for an unknown topic: {controller_topic}") + self._joint_publishers[controller_topic] = self.create_publisher( + JointTrajectory, controller_topic, qos_profile=VOLATILE_QOS) + + publisher = self._joint_publishers[controller_topic] + + msg = JointTrajectory() + msg.header.stamp = self.get_clock().now().to_msg() + msg.joint_names = joint_names # Must be in the correct order + + point = JointTrajectoryPoint() + point.positions = positions_rad # Must be in radians + # A small non-zero time is required for the controller to accept the goal + point.time_from_start = Duration(nanoseconds=200000000).to_msg() # 0.2s + + msg.points.append(point) + publisher.publish(msg) +``` + +## 3. Subscribing to State + +### Monitoring Connected Clients + +You can monitor how many clients are connected to the control system. This is useful for diagnostics and understanding system load. + +- **Topic:** `/client_count` +- **Message Type:** `std_msgs/msg/Int32` +- **QoS:** Latching (`TRANSIENT_LOCAL` durability) + +- **Service:** `/get_client_count` +- **Service Type:** `lucy_msgs/srv/GetInt` + +**Protocol:** +1. **On Startup:** Call the `/get_client_count` service to get the number of clients already connected. +2. **Continuously:** Subscribe to the `/client_count` topic to receive live updates whenever a client connects or disconnects. + +```python +# From lucy_cli/ros_interface.py + +# In __init__: +# self.create_subscription(Int32, '/client_count', self._client_count_callback, qos_profile=LATCHING_QOS) + +def _client_count_callback(self, msg: Int32): + """Handles incoming messages on the client count topic.""" + new_count = msg.data + # ... (update internal state and notify application) ... + +# --- In your startup logic --- +def get_initial_client_count(self) -> int | None: + """ + Synchronously calls the /get_client_count service. + """ + # ... (implementation similar to get_hardware_config_yaml) ... +``` + +### Other Important Topics + +- **`/joint_states`** (`sensor_msgs/msg/JointState`): This topic is published by `ros2_control` and contains the *actual*, measured position of every joint on the robot. You should subscribe to this to display the robot's real-time state, which may differ from your last commanded position. + +## Summary of Endpoints + +| Endpoint | Type | Message/Service Type | QoS Durability | Purpose | +|---|---|---|---|---| +| `/control_panel_active_client` | Topic | `std_msgs/msg/String` | `TRANSIENT_LOCAL` | Take, release, and monitor control. | +| `/client_count` | Topic | `std_msgs/msg/Int32` | `TRANSIENT_LOCAL` | Get live updates on the number of connected clients. | +| `/config/get` | Service | `lucy_msgs/srv/GetConfig` | N/A | Fetch the active robot hardware configuration (YAML). | +| `/get_client_count` | Service | `lucy_msgs/srv/GetInt` | N/A | Get the initial number of connected clients on startup. | +| `/{controller_name}/joint_trajectory` | Topic | `trajectory_msgs/msg/JointTrajectory` | `VOLATILE` | Send joint position commands to a specific controller. | +| `/joint_states` | Topic | `sensor_msgs/msg/JointState` | `VOLATILE` | Read the actual, measured state of all robot joints. | + +By using these endpoints, you can build any application to reliably and safely interact with the Lucy robot ecosystem. \ No newline at end of file diff --git a/lucy_cli/lucy_cli/.gitkeep b/lucy_cli/lucy_cli/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/lucy_cli/lucy_cli/__init__.py b/lucy_cli/lucy_cli/__init__.py new file mode 100644 index 0000000..a7603fd --- /dev/null +++ b/lucy_cli/lucy_cli/__init__.py @@ -0,0 +1,3 @@ +from . import ros_interface +from . import tui +from . import tui_node diff --git a/lucy_cli/lucy_cli/ros_interface.py b/lucy_cli/lucy_cli/ros_interface.py new file mode 100644 index 0000000..12afeb8 --- /dev/null +++ b/lucy_cli/lucy_cli/ros_interface.py @@ -0,0 +1,343 @@ +""" +This module provides a high-level, reusable Python interface for interacting +with Lucy's core ROS 2 services and topics. + +It is designed to be agnostic of any specific user interface (TUI, GUI, web), +allowing developers to build their own control applications by using this class +as a foundation. + +The LucyROSInterface class encapsulates all ROS 2-specific logic, including: +- Node and executor management. +- Service clients for fetching configuration and state. +- Publishers for sending commands. +- Subscribers for receiving state updates. +- Correct QoS (Quality of Service) profile management. + +Example Usage: + import rclpy + from ros_interface import LucyROSInterface + + def on_client_count_change(count): + print(f"Client count updated: {count}") + + def on_control_change(controller_id): + print(f"Control changed to: {controller_id}") + + rclpy.init() + ros_interface = LucyROSInterface() + ros_interface.register_on_client_count_change(on_client_count_change) + ros_interface.register_on_control_change(on_control_change) + + # The interface handles spinning in a background thread. + # You can now use its methods to interact with the robot. + + try: + config = ros_interface.get_hardware_config() + if config: + # ... parse config and build UI ... + pass + # Your application logic here... + finally: + ros_interface.shutdown() + rclpy.shutdown() +""" +import threading +import rclpy +import time +import random +from rclpy.node import Node +from rclpy.duration import Duration +from rclpy.qos import QoSProfile, QoSHistoryPolicy, QoSReliabilityPolicy, QoSDurabilityPolicy +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_msgs.msg import String, Int32 +from lucy_msgs.srv import GetConfig, GetInt + +# --- Unique Client ID --- +# A unique identifier for this specific client instance. +CLIENT_ID = f"cli_{int(time.time())}_{random.randint(1000, 9999)}" + +# --- Topic Names --- +CONTROL_MODE_TOPIC = '/control_panel_active_client' +CLIENT_COUNT_TOPIC = '/client_count' + +# --- QoS Profiles --- +# For state topics that should be "latched" (i.e., the last published message +# is saved for new subscribers). This is crucial for getting the current +# controller and client count as soon as a client connects. +LATCHING_QOS = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL +) +# For command topics where you only care about the latest command and don't +# need persistence. This is the standard for ros2_control joint trajectories. +VOLATILE_QOS = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.VOLATILE +) + + +class LucyROSInterface(Node): + """ + A ROS 2 node providing a high-level API to interact with Lucy's systems. + + This class manages all ROS 2 publishers, subscribers, and service clients, + exposing simple methods and callbacks for use in an application. It handles + its own ROS executor in a background thread, so the user does not need to + manually call `rclpy.spin()`. + """ + + def __init__(self): + super().__init__('lucy_ros_interface_node') + + # --- Internal State --- + self._active_controller_id = "" + self._client_count = 0 + self._has_control = False + self._control_lock = threading.Lock() + self._is_shutting_down = False + + # --- Callbacks --- + # These sets will hold functions to be called when state changes. + self._on_control_change_callbacks = set() + self._on_client_count_change_callbacks = set() + + # --- Publishers --- + self.control_publisher = self.create_publisher( + String, CONTROL_MODE_TOPIC, qos_profile=LATCHING_QOS) + # Joint publishers are created dynamically after config is loaded. + self._joint_publishers = {} + + # --- Subscribers --- + self.create_subscription( + String, CONTROL_MODE_TOPIC, self._control_callback, qos_profile=LATCHING_QOS) + self.create_subscription( + Int32, CLIENT_COUNT_TOPIC, self._client_count_callback, qos_profile=LATCHING_QOS) + + # --- Service Clients --- + self._get_config_client = self.create_client(GetConfig, '/config/get') + self._get_count_client = self.create_client(GetInt, '/get_client_count') + + # --- Executor --- + # Spin the node in a background thread to handle callbacks automatically. + self._executor = rclpy.executors.MultiThreadedExecutor() + self._executor.add_node(self) + self._spin_thread = threading.Thread(target=self._executor.spin, daemon=True) + self._spin_thread.start() + + self.get_logger().info(f"LucyROSInterface started with Client ID: {CLIENT_ID}") + # Take control on startup + self.take_control() + + # --- Public Methods: Callbacks --- + + def register_on_control_change(self, callback): + """ + Register a function to be called when the active controller changes. + + The callback will receive one argument: + - active_client_id (str): The ID of the client now in control, or an + empty string if control has been released. + """ + self._on_control_change_callbacks.add(callback) + + def register_on_client_count_change(self, callback): + """ + Register a function to be called when the client count changes. + + The callback will receive one argument: + - count (int): The new number of connected clients. + """ + self._on_client_count_change_callbacks.add(callback) + + # --- Public Methods: State & Commands --- + + @property + def client_id(self) -> str: + """Returns the unique ID for this client instance.""" + return CLIENT_ID + + def has_control(self) -> bool: + """Returns True if this client currently has control, False otherwise.""" + with self._control_lock: + return self._has_control + + def take_control(self): + """Publishes this client's ID to request control of the robot.""" + if self._is_shutting_down: return + self.get_logger().info("Requesting control...") + msg = String(data=CLIENT_ID) + try: + self.control_publisher.publish(msg) + except Exception: + pass # Ignore errors if already shutting down + + def release_control(self): + """Publishes an empty string to release control of the robot.""" + if self._is_shutting_down: return + self.get_logger().info("Releasing control...") + msg = String(data="") + try: + self.control_publisher.publish(msg) + except Exception: + pass # Ignore errors if already shutting down + + def publish_joint_trajectory(self, controller_topic: str, joint_names: list[str], positions_rad: list[float]): + """ + Publishes a JointTrajectory message to a specified controller. + + Args: + controller_topic: The full topic name for the joint trajectory + controller (e.g., '/left_arm_controller/joint_trajectory'). + joint_names: A list of joint names in the correct order for the controller. + positions_rad: A list of target joint positions in radians, matching + the order of joint_names. + """ + if self._is_shutting_down: return + if controller_topic not in self._joint_publishers: + self.get_logger().warn(f"Creating new publisher for an unknown topic: {controller_topic}") + self._joint_publishers[controller_topic] = self.create_publisher( + JointTrajectory, controller_topic, qos_profile=VOLATILE_QOS) + + publisher = self._joint_publishers[controller_topic] + msg = JointTrajectory() + msg.header.stamp = self.get_clock().now().to_msg() + msg.joint_names = joint_names + point = JointTrajectoryPoint() + point.positions = positions_rad + point.time_from_start = Duration(nanoseconds=200000000).to_msg() # 0.2s + msg.points.append(point) + try: + publisher.publish(msg) + self.get_logger().info(f"Published JointTrajectory to {controller_topic}") + except Exception: + pass + + # --- Public Methods: Initial State Fetching --- + + def get_hardware_config_yaml(self) -> str | None: + """ + Synchronously calls the /config/get service to fetch the active config. + + This is a blocking call and should be used during application startup. + + Returns: + The active hardware configuration as a YAML string, or None if + the service call fails or times out. + """ + try: + if not self._get_config_client.wait_for_service(timeout_sec=5.0): + self.get_logger().error("'/config/get' service not available.") + return None + req = GetConfig.Request(config_name="") + future = self._get_config_client.call_async(req) + + # Create a temporary node solely to wait for THIS specific future. + # This avoids interfering with the main executor. + temp_node = Node('temp_service_caller') + rclpy.spin_until_future_complete(temp_node, future, timeout_sec=5.0) + temp_node.destroy_node() + + response = future.result() + if not response or not response.success: + self.get_logger().error(f"Failed to get active config: {response.message if response else 'timeout'}") + return None + self.get_logger().info("Successfully fetched active hardware configuration.") + return response.config_yaml + except Exception as e: + self.get_logger().error(f"Exception in get_hardware_config_yaml: {e}") + return None + + def get_initial_client_count(self) -> int | None: + """ + Synchronously calls the /get_client_count service. + + This is a blocking call and should be used during application startup. + + Returns: + The current client count as an integer, or None on failure. + """ + try: + if not self._get_count_client.wait_for_service(timeout_sec=3.0): + self.get_logger().warn("'/get_client_count' service not available.") + return None + req = GetInt.Request() + future = self._get_count_client.call_async(req) + + # Create a temporary node solely to wait for THIS specific future. + # This avoids interfering with the main executor. + temp_node = Node('temp_service_caller_2') + rclpy.spin_until_future_complete(temp_node, future, timeout_sec=3.0) + temp_node.destroy_node() + + response = future.result() + if response: + self.get_logger().info(f"Fetched initial client count: {response.value}") + # Manually update internal state and call callbacks + self._client_count = response.value + for cb in self._on_client_count_change_callbacks: + cb(self._client_count) + return response.value + return None + except Exception as e: + self.get_logger().error(f"Exception in get_initial_client_count: {e}") + return None + + # --- Internal ROS Callbacks --- + + def _control_callback(self, msg: String): + """Handles incoming messages on the control topic.""" + with self._control_lock: + new_controller_id = msg.data + + # Only react if the controller has actually changed + if self._active_controller_id == new_controller_id: + return + + self._active_controller_id = new_controller_id + + # We only lose control if someone else took it. + # We do NOT lose control if the message is from ourselves, + # nor do we gain control if the message is empty (released). + if new_controller_id == CLIENT_ID: + self._has_control = True + self.get_logger().info("This client successfully took control.") + elif new_controller_id == "": + self._has_control = False + self.get_logger().info("Control was released by previous client.") + else: + self._has_control = False + self.get_logger().info(f"Control taken by another client: '{new_controller_id}'") + + # Notify all registered listeners of the change + for cb in self._on_control_change_callbacks: + cb(new_controller_id) + + def _client_count_callback(self, msg: Int32): + """Handles incoming messages on the client count topic.""" + with self._control_lock: + new_count = msg.data + if self._client_count != new_count: + self._client_count = new_count + self.get_logger().info(f"Client count updated: {new_count}") + # Notify all registered listeners + for cb in self._on_client_count_change_callbacks: + cb(new_count) + + # --- Lifecycle --- + + def shutdown(self): + """Gracefully shuts down the node and its background thread.""" + if self._is_shutting_down: return + self._is_shutting_down = True + + self.get_logger().info("Shutting down LucyROSInterface.") + self.release_control() + # Give a moment for the release message to be sent + time.sleep(0.1) + self._executor.shutdown() + # self._spin_thread.join() # This can hang, shutdown is enough + self.destroy_node() \ No newline at end of file diff --git a/lucy_cli/lucy_cli/tui.py b/lucy_cli/lucy_cli/tui.py new file mode 100644 index 0000000..7e51536 --- /dev/null +++ b/lucy_cli/lucy_cli/tui.py @@ -0,0 +1,176 @@ +""" +This module contains all the logic for rendering the Text-based User Interface (TUI). + +It is designed to be completely decoupled from ROS or any other backend logic. +It receives data structures to display and uses callbacks to notify the main +application of user input. + +Responsibilities: +- Clearing the screen. +- Drawing menus and lists. +- Handling raw keyboard input with timeouts. +- Maintaining a persistent input buffer across screen refreshes. +""" +import os +import sys +import select +import time + +# Global buffer to hold unsubmitted user input, allowing it to persist +# across screen refreshes in auto-refresh mode. +INPUT_BUFFER = "" + +def clear_screen(): + """Clears the terminal screen using ANSI escape codes for a flicker-free update.""" + if os.name == 'posix': + sys.stdout.write("\033[H\033[J") + sys.stdout.flush() + else: + os.system('cls') + +def get_user_input(prompt: str, timeout: float = 1.0) -> str | None: + """ + Waits for user input with a timeout, preserving typed characters. + + This function uses non-blocking, character-by-character input on POSIX + systems to provide a smooth TUI experience, even when the screen is + auto-refreshing. + + Args: + prompt: The input prompt to display to the user. + timeout: The time in seconds to wait for input before returning. + + Returns: + - The user's full line of input if they press Enter. + - An empty string ("") if the user typed but did not press Enter. + - None if the timeout was reached with no user input. + """ + global INPUT_BUFFER + sys.stdout.write(prompt + INPUT_BUFFER) + sys.stdout.flush() + + if os.name != 'posix': + # Fallback for non-POSIX systems (like Windows) which lacks tty/termios. + # This will not have the non-blocking, char-by-char benefits. + res = input() + return res + + import termios, tty + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setcbreak(fd) + if select.select([sys.stdin], [], [], timeout)[0]: + while True: + char = sys.stdin.read(1) + if char == '\n' or char == '\r': # Enter pressed + result = INPUT_BUFFER + INPUT_BUFFER = "" + sys.stdout.write('\n') + return result + elif char == '\x7f' or char == '\b': # Backspace + if INPUT_BUFFER: + INPUT_BUFFER = INPUT_BUFFER[:-1] + # Move cursor back, write space, move back again + sys.stdout.write('\b \b') + sys.stdout.flush() + elif char == '\x03': # Ctrl+C + raise KeyboardInterrupt + elif char.isprintable(): + INPUT_BUFFER += char + sys.stdout.write(char) + sys.stdout.flush() + + # Check if more characters are available to be read immediately + if not select.select([sys.stdin], [], [], 0)[0]: + break + # User typed, but didn't press Enter. Redraw will handle the buffer. + return "" + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + + # Timeout occurred + return None + +def display_help_screen(): + """Displays a static help message and waits for user confirmation.""" + clear_screen() + print("--- TUI Help ---") + print("\n[General Commands]") + print(" 'a' - Toggle auto-refresh mode on/off.") + print(" 'h' - Display this help screen.") + print(" 'q' - Quit the application.") + print("\n[Main Menu]") + print(" - Select an actuator group to view and edit its joints.") + print(" 't' - (When available) Forcibly take control from another client.") + print("\n[Joint Menu]") + print(" - Select a joint to modify its angle.") + print(" 'b' - Go back to the main menu.") + print("\nPress Enter to return...") + input() + +def display_main_menu(state: dict): + """ + Renders the main menu screen. + + Args: + state: A dictionary containing the current UI state, including: + 'client_count', 'autorefresh', 'has_control', 'active_controller', + and 'actuator_groups'. + """ + print("--- Robot Actuator TUI ---") + print(f"Control Panel Connection Count: {state.get('client_count', 'N/A')}") + if state.get('autorefresh'): + print("[Auto-Refresh: ON]") + + if not state.get('has_control') and state.get('active_controller'): + print(f"!! CONTROLLED BY: {state['active_controller']} !!\n") + print("Press 't' to take control.") + + print("\nSelect an actuator group:") + for i, name in enumerate(state.get('actuator_groups', [])): + print(f"{i+1}. {name}") + print("\nEnter 'q' to quit or 'h' for help.") + +def display_joint_menu(state: dict, group_name: str): + """ + Renders the menu for editing joints within a specific group. + + Args: + state: A dictionary containing the current UI state. + group_name: The name of the actuator group being displayed. + """ + print(f"--- {group_name} ---") + if not state.get('has_control'): + print(f"!! READ-ONLY (Controlled by {state.get('active_controller')}) !!\n") + + joints = state.get('actuators', {}).get(group_name, {}).get('joints', []) + for i, joint in enumerate(joints): + print(f"{i+1}. {joint['name']}: {joint['value']:.1f}° (Min: {joint['min']:.0f}, Max: {joint['max']:.0f})") + + if state.get('has_control'): + print("\nEnter joint number to edit, 'b' to go back, 'h' for help, or 'q' to quit.") + else: + print("\nEnter 'b' to go back, 'h' for help, or 'q' to quit.") + +def get_new_joint_value(joint_name: str) -> float | None: + """ + Prompts the user for a new joint value. + + This uses standard blocking input as it's a specific data entry task. + + Args: + joint_name: The name of the joint being edited. + + Returns: + The new value as a float, or None if input is invalid. + """ + global INPUT_BUFFER + INPUT_BUFFER = "" # Clear buffer before this prompt + try: + value_str = input(f"Enter new value for {joint_name} (in degrees): ") + return float(value_str) + except (ValueError, TypeError): + print("Invalid input. Please enter a number.") + time.sleep(1) + return None \ No newline at end of file diff --git a/lucy_cli/lucy_cli/tui_node.py b/lucy_cli/lucy_cli/tui_node.py new file mode 100644 index 0000000..1db203f --- /dev/null +++ b/lucy_cli/lucy_cli/tui_node.py @@ -0,0 +1,207 @@ +""" +The main entry point for the Lucy CLI TUI Actuator Node. +... +""" +import sys +import argparse +import time +import rclpy +import math +import yaml +from .ros_interface import LucyROSInterface +from .tui import ( + clear_screen, get_user_input, display_help_screen, + display_main_menu, display_joint_menu, get_new_joint_value +) + +def run_tui(ros: LucyROSInterface, actuators: dict, autorefresh: bool): + """ + The main application loop. + + Args: + ros: The initialized LucyROSInterface instance. + actuators: The dynamic actuator configuration dictionary. + autorefresh: Boolean indicating if auto-refresh is enabled by default. + """ + current_autorefresh = autorefresh + + while rclpy.ok(): + clear_screen() + + state = { + 'autorefresh': current_autorefresh, + 'client_count': ros._client_count, + 'has_control': ros.has_control(), + 'active_controller': ros._active_controller_id, + 'actuator_groups': list(actuators.keys()), + 'actuators': actuators + } + + display_main_menu(state) + + timeout = 1.0 if state['autorefresh'] else 3600.0 + choice = get_user_input("> ", timeout=timeout) + + if choice is None or choice == '': + continue + + choice = choice.lower() + if choice == 'q': + break + elif choice == 'h': + display_help_screen() + elif choice == 'a': + current_autorefresh = not current_autorefresh + print(f"Auto-refresh {'enabled' if current_autorefresh else 'disabled'}.") + time.sleep(0.7) + elif choice == 't' and not state['has_control']: + ros.take_control() + print("Requesting control...") + time.sleep(0.5) + else: + try: + idx = int(choice) - 1 + if 0 <= idx < len(state['actuator_groups']): + group_name = state['actuator_groups'][idx] + current_autorefresh = handle_group_menu(ros, actuators, group_name, current_autorefresh) + else: + raise ValueError + except ValueError: + print("Invalid input. Refreshing...") + time.sleep(1) + +def handle_group_menu(ros: LucyROSInterface, actuators: dict, group_name: str, autorefresh: bool) -> bool: + """Handles the sub-menu for a specific actuator group.""" + current_autorefresh = autorefresh + + while rclpy.ok(): + clear_screen() + + state = { + 'autorefresh': current_autorefresh, + 'client_count': ros._client_count, + 'has_control': ros.has_control(), + 'active_controller': ros._active_controller_id, + 'actuator_groups': list(actuators.keys()), + 'actuators': actuators + } + + display_joint_menu(state, group_name) + + timeout = 1.0 if state['autorefresh'] else 3600.0 + choice = get_user_input("> ", timeout=timeout) + + if choice is None or choice == '': + continue + + choice = choice.lower() + if choice == 'b': + break + elif choice == 'q': + ros.shutdown() + rclpy.try_shutdown() + sys.exit(0) + elif choice == 'h': + display_help_screen() + elif choice == 'a': + current_autorefresh = not current_autorefresh + print(f"Auto-refresh {'enabled' if current_autorefresh else 'disabled'}.") + time.sleep(0.7) + elif state['has_control']: + try: + joints = state['actuators'][group_name]['joints'] + joint_idx = int(choice) - 1 + if 0 <= joint_idx < len(joints): + selected_joint = joints[joint_idx] + new_val = get_new_joint_value(selected_joint['name']) + if new_val is not None: + if selected_joint['min'] <= new_val <= selected_joint['max']: + selected_joint['value'] = new_val + + names = [j['name'] for j in joints] + positions = [math.radians(j['value']) for j in joints] + topic = state['actuators'][group_name]['topic'] + + ros.publish_joint_trajectory(topic, names, positions) + print("Value updated successfully. Refreshing...") + else: + print(f"Value out of range ({selected_joint['min']} - {selected_joint['max']}).") + time.sleep(1) + else: + raise ValueError + except ValueError: + print("Invalid input.") + time.sleep(1) + + return current_autorefresh + +def parse_config_yaml(yaml_string: str) -> dict: + """Parses the hardware config YAML and builds the ACTUATORS dictionary.""" + if not yaml_string: return {} + try: + doc = yaml.safe_load(yaml_string) + if not isinstance(doc, dict): return {} + boards, actuators_list = doc.get('boards', {}), doc.get('actuators', []) + parsed_actuators = {} + for board_id, bdef in boards.items(): + if not isinstance(bdef, dict): continue + ctrl_name = bdef.get('controller', {}).get('name') + if not ctrl_name: continue + board_actuators = sorted([a for a in actuators_list if isinstance(a, dict) and a.get('board') == board_id], key=lambda a: a.get('virtual_pin', 0)) + joints = [] + for actuator in board_actuators: + urdf_joint = actuator.get('urdf_joint') + if not urdf_joint: continue + joints.append({ + "name": urdf_joint, + "min": float(actuator.get('servo_min_deg', 0.0)), + "max": float(actuator.get('servo_max_deg', 180.0)), + "value": float(actuator.get('servo_default_deg', 90.0)), + }) + if joints: + group_name = board_id.replace("rp2040_", "").replace("_", " ").title() + parsed_actuators[group_name] = {"topic": f"/{ctrl_name}/joint_trajectory", "joints": joints} + return parsed_actuators + except yaml.YAMLError as e: + print(f"Error parsing YAML: {e}") + return {} + +def main(args=None): + parser = argparse.ArgumentParser(description='Lucy TUI Actuator Node') + parser.add_argument('-a', '--autorefresh', action='store_true', help='Enable auto-refresh for the TUI screen') + + sys_args = sys.argv[1:] if args is None else args + parsed_args, ros_args = parser.parse_known_args(sys_args) + + rclpy.init(args=ros_args) + ros_interface = LucyROSInterface() + + print("Fetching active hardware configuration...") + config_yaml = ros_interface.get_hardware_config_yaml() + if not config_yaml: + print("Could not retrieve hardware configuration. Is lucy_config_pipeline running?") + ros_interface.shutdown() + rclpy.shutdown() + sys.exit(1) + + actuators = parse_config_yaml(config_yaml) + if not actuators: + print("Failed to parse hardware configuration or no actuators found.") + ros_interface.shutdown() + rclpy.shutdown() + sys.exit(1) + + print("Fetching initial client count...") + ros_interface.get_initial_client_count() + + try: + run_tui(ros_interface, actuators, parsed_args.autorefresh) + except KeyboardInterrupt: + pass + finally: + print("\nShutting down...") + ros_interface.shutdown() + rclpy.try_shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/lucy_cli/package.xml b/lucy_cli/package.xml new file mode 100644 index 0000000..9590966 --- /dev/null +++ b/lucy_cli/package.xml @@ -0,0 +1,23 @@ + + + + lucy_cli + 0.0.0 + CLI package for Lucy + Mael-RABOT + GPL-3.0 + + rclpy + lucy_msgs + sensor_msgs + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/lucy_cli/resource/lucy_cli b/lucy_cli/resource/lucy_cli new file mode 100644 index 0000000..e69de29 diff --git a/lucy_cli/setup.cfg b/lucy_cli/setup.cfg new file mode 100644 index 0000000..33bb8de --- /dev/null +++ b/lucy_cli/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lucy_cli +[install] +install_scripts=$base/lib/lucy_cli \ No newline at end of file diff --git a/lucy_cli/setup.py b/lucy_cli/setup.py new file mode 100644 index 0000000..c5eada6 --- /dev/null +++ b/lucy_cli/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'lucy_cli' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Mael-RABOT', + maintainer_email='contact@sentience-robotics.fr', + description='CLI package for Lucy', + license='GPL-3.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'tui = lucy_cli.tui_node:main' + ], + }, +) \ No newline at end of file diff --git a/lucy_config_generator/.gitkeep b/lucy_config_generator/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/lucy_config_generator/README.md b/lucy_config_generator/README.md new file mode 100644 index 0000000..dcc41e8 --- /dev/null +++ b/lucy_config_generator/README.md @@ -0,0 +1,66 @@ +# lucy_config_generator + +Python tool that reads the hardware mapping YAML from an **URDF** (`config/hardware/active.yaml`) and generates: + +1. **Firmware** — one `config_.c` per RP2040 board (filename equals the **`boards:`** key, e.g. `config_rp2040_left_arm.c`), suitable for `micro_ros_raspberrypi_pico_sdk` (`constant.h` / `INTERNAL_SERVO_*`, `SERVO_TYPE_*`, `DEG_TO_RAD`). +2. **ros2_control** — `inmoov_ros2_control.xacro` with one `` block per board in **`boards:`** document order; `hardware_name` / `node_name` / `publisher_topic` are **derived and validated** from each board id and YAML fields (see `schema.py`). Shoulder Y joints stay on whichever board the YAML assigns them to (typically `rp2040_torso_head`). Each actuated joint also gets `` populated from the URDF `` (radians) — these are the values `LucySystemHardware` clamps to in `write()`. +3. **Controllers** — `controllers.yaml` with `joint_state_broadcaster` / trajectory controllers and **`extra_joints`** as *URDF joints that are not any YAML actuator* (passive / unmapped only; all actuator rows are trajectory-controlled regardless of `enabled`). + +Schema validation lives in `lucy_config_generator/schema.py` for reuse by **`lucy_config_pipeline`**. + +## Generated filenames (`generated_files`) + +The ros2_control xacro and `controllers.yaml` basenames are centralized in the hardware YAML so the generator, **`lucy_config_pipeline`**, `inmoov.urdf.xacro` and the **`thais_urdf`** launches all agree. The section is **optional**; omitting it (or any key) falls back to `schema.GENERATED_FILES_DEFAULTS`. + +```yaml +generated_files: + ros2_control_xacro: inmoov_ros2_control.xacro # written to description/ros2_control/ + controllers_yaml: controllers.yaml # written to config/ +``` + +Values are **bare filenames** (no path separators); the directories are fixed by repo convention. `inmoov.urdf.xacro` reads the basename via its `ros2_control_file` xacro arg, and the launches/supervisor forward the same value so the running stack loads the file the pipeline just installed. Resolve them in code with `schema.resolve_generated_files(data)`. + +## CLI + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash + +generate_config \ + --input src/thais_urdf/config/hardware/active.yaml \ + --urdf src/thais_urdf/description/urdf/inmoov.urdf.xacro \ + --base-path src/thais_urdf/description \ + --controller-config src/thais_urdf/config/controllers.yaml \ + --output-dir /tmp/gen_out \ + --targets all +``` + +Options: + +| Flag | Meaning | +|------|---------| +| `--targets` | `all` (default), `firmware`, `ros2_control`, or `controllers` | +| `--boards` | Optional comma-separated board ids (e.g. `rp2040_left_arm,rp2040_torso_head`) | + +`--urdf`, `--base-path`, and `--controller-config` are passed to **`xacro`** the same way as **`thais_urdf`** tests so joint discovery matches the real robot. + +## Templates + +Jinja2 sources are under the Python package at `lucy_config_generator/lucy_config_generator/templates/`: + +| Template | Output | +|----------|--------| +| `config_internal_only_board.c.j2` | `board_class: internal_servo_only` — single internal PWM `dump_config()` | +| `config_internal_i2c_board.c.j2` | `board_class: internal_servo_i2c_pwm` — internal PWM + I2C/PCA scaffold | +| `ros2_control.xacro.j2` | One `` block per board (names derived from board id). Selects `gz_ros2_control/GazeboSimSystem` when `use_gazebo_sim:=true`, otherwise `lucy_ros2_control/LucySystemHardware` for both real and mock (mock sets `false` so no micro-ROS traffic, URDF clamping still runs). | +| `controllers.yaml.j2` | `controller_manager` + per-board joint lists + `extra_joints` | + +**Disabled actuators** (`enabled: false`): **omitted from firmware C** only (no `virtual_pin` row on the Pico for that joint). They **remain** in **`ros2_control`** and in the per-board **trajectory controller** joint list so the stack is uniform; firmware ignores commands at `virtual_pin` indices it does not own. **`extra_joints`** lists only URDF joints that are **not** any actuator row (typically passive links). + +## Tests + +```bash +colcon test --packages-select lucy_config_generator --event-handlers console_direct+ +``` + +Golden files live in `test/fixtures/` alongside `test_mapping.yaml`. `test/test_position_limits_unified.py` pins that URDF position limits land on every actuator's `command_interface` and stay outside any `` so every plugin path receives them identically. \ No newline at end of file diff --git a/lucy_config_generator/lucy_config_generator/.gitkeep b/lucy_config_generator/lucy_config_generator/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/lucy_config_generator/lucy_config_generator/__init__.py b/lucy_config_generator/lucy_config_generator/__init__.py new file mode 100644 index 0000000..9ec3dcf --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/__init__.py @@ -0,0 +1,5 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Generate firmware C, ros2_control xacro, and controllers YAML from hardware YAML (#96).""" diff --git a/lucy_config_generator/lucy_config_generator/generate.py b/lucy_config_generator/lucy_config_generator/generate.py new file mode 100644 index 0000000..34af4d5 --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/generate.py @@ -0,0 +1,506 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Jinja2 rendering for firmware, ros2_control, and controllers.""" + +from __future__ import annotations + +import shutil +import subprocess +import xml.etree.ElementTree as ET +from typing import Any +from pathlib import Path + +import jinja2 +import yaml + +from lucy_config_generator.schema import ( + BOARD_CLASS_INTERNAL_I2C_PWM, + BOARD_CLASS_INTERNAL_ONLY, + derive_ros2_hardware_name, + derive_ros2_node_name, + resolve_generated_files, + validate_hardware_yaml, +) + + +def _templates_dir() -> Path: + return Path(__file__).resolve().parent / "templates" + + +def _jinja_env() -> jinja2.Environment: + return jinja2.Environment( + loader=jinja2.FileSystemLoader(str(_templates_dir())), + autoescape=False, + trim_blocks=True, + lstrip_blocks=True, + ) + + +def load_hardware_yaml(path: Path) -> dict[str, Any]: + with path.open(encoding="utf-8") as f: + data = yaml.safe_load(f) + if not isinstance(data, dict): + raise ValueError("YAML root must be a mapping") + return data + + +def _xacro_argv( + urdf_xacro: Path, base_path: Path, controller_config: Path +) -> list[str]: + """Prefer ``ros2 run xacro xacro`` so broken distro ``xacro`` console scripts still work.""" + tail = [ + str(urdf_xacro), + f"base_path:={base_path}", + f"controller_config:={controller_config}", + "use_gazebo_sim:=false", + ] + if shutil.which("ros2") is not None: + return ["ros2", "run", "xacro", "xacro", *tail] + if shutil.which("xacro") is not None: + return ["xacro", *tail] + raise RuntimeError( + "Need 'ros2' (for `ros2 run xacro xacro`) " "or `xacro` on PATH to process URDF" + ) + + +def _parse_urdf_joints_xml( + urdf_xml: str, +) -> tuple[set[str], dict[str, tuple[float, float]]]: + """Joint names and URDF ```` (rad) for revolute/prismatic joints.""" + root = ET.fromstring(urdf_xml) + names: set[str] = set() + limits: dict[str, tuple[float, float]] = {} + for j in root.findall("joint"): + name = j.attrib.get("name") + if not name: + continue + names.add(name) + if j.attrib.get("type") not in ("revolute", "prismatic"): + continue + limit_el = j.find("limit") + if limit_el is None: + continue + lo = limit_el.attrib.get("lower") + hi = limit_el.attrib.get("upper") + if lo is None or hi is None: + continue + limits[name] = (float(lo), float(hi)) + return names, limits + + +def _expand_urdf_xacro( + urdf_xacro: Path, + base_path: Path, + controller_config: Path, +) -> str: + if not urdf_xacro.is_file(): + raise FileNotFoundError(urdf_xacro) + if not base_path.is_dir(): + raise FileNotFoundError(base_path) + if not controller_config.is_file(): + raise FileNotFoundError(controller_config) + cmd = _xacro_argv(urdf_xacro, base_path, controller_config) + r = subprocess.run(cmd, capture_output=True, text=True, timeout=120, check=False) + if r.returncode != 0: + raise RuntimeError(f"xacro failed: {r.stderr}") + return r.stdout + + +def urdf_joint_names( + urdf_xacro: Path, + base_path: Path, + controller_config: Path, +) -> set[str]: + """Joint names from processed URDF (same args as thais_urdf xacro smoke).""" + names, _ = _parse_urdf_joints_xml( + _expand_urdf_xacro(urdf_xacro, base_path, controller_config) + ) + return names + + +def urdf_joint_limits( + urdf_xacro: Path, + base_path: Path, + controller_config: Path, +) -> dict[str, tuple[float, float]]: + """URDF position limits (rad) keyed by joint name.""" + _, limits = _parse_urdf_joints_xml( + _expand_urdf_xacro(urdf_xacro, base_path, controller_config) + ) + return limits + + +def _board_ids_in_yaml_order(data: dict[str, Any]) -> list[str]: + """Preserve ``boards:`` key order from YAML (PyYAML + Python 3.7+ dict order).""" + boards = data["boards"] + if not isinstance(boards, dict): + raise ValueError("boards must be a mapping") + return list(boards.keys()) + + +def _actuators_for_board( + data: dict[str, Any], board_id: str, *, enabled_only: bool +) -> list[dict[str, Any]]: + out: list[dict[str, Any]] = [] + for a in data["actuators"]: + if a["board"] != board_id: + continue + if enabled_only and not a.get("enabled", True): + continue + out.append(a) + out.sort(key=lambda x: int(x["virtual_pin"])) + return out + + +def _sensors_for_board(data: dict[str, Any], board_id: str) -> list[dict[str, Any]]: + out = [s for s in data["sensors"] if s["board"] == board_id] + out.sort(key=lambda x: int(x["virtual_pin"])) + return out + + +def _sensors_for_board_firmware( + data: dict[str, Any], board_id: str +) -> list[dict[str, Any]]: + """Pressure rows only if their associated actuator is enabled (matches firmware C scope).""" + enabled_ids = {a["id"] for a in data["actuators"] if a.get("enabled", True)} + out = [ + s + for s in data["sensors"] + if s["board"] == board_id and s["associated_actuator"] in enabled_ids + ] + out.sort(key=lambda x: int(x["virtual_pin"])) + return out + + +def _actuator_joint_for_ros2( + actuator: dict[str, Any], + urdf_limits: dict[str, tuple[float, float]], +) -> dict[str, Any]: + """Copy actuator row and attach URDF command_interface min/max when present.""" + row = dict(actuator) + pair = urdf_limits.get(actuator["urdf_joint"]) + if pair is not None: + row["limit_lower_rad"] = pair[0] + row["limit_upper_rad"] = pair[1] + return row + + +def _ros2_control_blocks( + data: dict[str, Any], + board_ids: list[str], + urdf_limits: dict[str, tuple[float, float]], +) -> list[dict[str, Any]]: + blocks: list[dict[str, Any]] = [] + for bid in board_ids: + bdef = data["boards"][bid] + joints = [ + _actuator_joint_for_ros2(a, urdf_limits) + for a in _actuators_for_board(data, bid, enabled_only=False) + ] + blocks.append( + { + "board_id": bid, + "hardware_name": derive_ros2_hardware_name(bid), + "publisher_topic": bdef["topic_actuators"], + "node_name": derive_ros2_node_name(bid), + "joints": joints, + } + ) + return blocks + + +def _gazebo_camera_entry(camera: dict[str, Any]) -> dict[str, Any] | None: + """One Gazebo-sim camera for bridge/republish, or None when not simulated.""" + if not isinstance(camera, dict): + return None + + topic = camera.get("topic") + if not isinstance(topic, str) or not topic.strip(): + return None + topic = topic.strip() + + compressed_topic = camera.get("compressed_topic") + if not isinstance(compressed_topic, str) or not compressed_topic.strip(): + return None + compressed_topic = compressed_topic.strip() + + external = bool(camera.get("external")) + if external: + gz_topic = camera.get("sim_gz_topic") + if not isinstance(gz_topic, str) or not gz_topic.strip(): + return None + gz_topic = gz_topic.strip() + link = None + else: + link = camera.get("link") + if not isinstance(link, str) or not link.strip(): + return None + gz_topic = topic + link = link.strip() + return { + "name": camera["name"], + "topic": topic, + "compressed_topic": compressed_topic, + "gz_topic": gz_topic, + "message_type": camera["message_type"], + "external": external, + "link": link, + } + + +def _gazebo_bridge_cameras(data: dict[str, Any]) -> list[dict[str, Any]]: + """Cameras bridged in sim (robot-mounted + external world cameras).""" + cameras: list[dict[str, Any]] = [] + if "cameras" not in data: + return cameras + for camera in data["cameras"]: + entry = _gazebo_camera_entry(camera) + if entry is not None: + cameras.append(entry) + return cameras + + +def _gazebo_xacro_cameras(data: dict[str, Any]) -> list[dict[str, Any]]: + """Robot-mounted gz camera sensors only (excludes external/world cameras).""" + return [c for c in _gazebo_bridge_cameras(data) if not c["external"]] + + +def _gazebo_sensors(data: dict[str, Any]) -> list[dict[str, Any]]: + sensors: list[dict[str, Any]] = [] + if "sensors" not in data: + return sensors + + tmp_dict = {} + for sensor in data["sensors"]: + board_name = sensor["board"] + tmp_dict.setdefault(board_name, []).append({}) + + for board_name, board_data in data["boards"].items(): + if board_name not in tmp_dict: + continue + sensors.append( + {"topic": board_data["topic_sensors"], "sensors": tmp_dict[board_name]} + ) + return sensors + + +def _extra_joints(data: dict[str, Any], urdf_joints: set[str]) -> list[str]: + """ + Joints published at default via broadcaster, not listed on Lucy hardware blocks. + + Every actuator (enabled or not) is exported under ``ros2_control`` and trajectory + controllers; only **non-actuator** URDF joints (passive / unmapped) need + ``extra_joints`` so ``joint_state_broadcaster`` can publish them for TF. + """ + actuated = {a["urdf_joint"] for a in data["actuators"]} + extra = sorted(urdf_joints - actuated) + return extra + + +def _firmware_template_for_board_class(board_class: str) -> str: + if board_class == BOARD_CLASS_INTERNAL_I2C_PWM: + return "config_internal_i2c_board.c.j2" + if board_class == BOARD_CLASS_INTERNAL_ONLY: + return "config_internal_only_board.c.j2" + raise ValueError(f"unknown board_class for firmware template: {board_class!r}") + + +def render_firmware_c( + data: dict[str, Any], + board_id: str, + env: jinja2.Environment | None = None, +) -> str: + env = env or _jinja_env() + actuators = _actuators_for_board(data, board_id, enabled_only=True) + sensors = _sensors_for_board_firmware(data, board_id) + board_class = data["boards"][board_id]["board_class"] + tpl_name = _firmware_template_for_board_class(board_class) + tpl = env.get_template(tpl_name) + return tpl.render( + board_id=board_id, + actuators=actuators, + sensors=sensors, + ) + + +def render_ros2_control_xacro( + data: dict[str, Any], + board_ids: list[str], + urdf_limits: dict[str, tuple[float, float]], + env: jinja2.Environment | None = None, +) -> str: + env = env or _jinja_env() + tpl = env.get_template("ros2_control.xacro.j2") + return tpl.render(blocks=_ros2_control_blocks(data, board_ids, urdf_limits)) + + +def render_gazebo_xacro( + data: dict[str, Any], + board_ids: list[str], + urdf_limits: dict[str, tuple[float, float]], + env: jinja2.Environment | None = None, +) -> str: + env = env or _jinja_env() + tpl = env.get_template("gazebo.xacro.j2") + return tpl.render( + blocks=_ros2_control_blocks(data, board_ids, urdf_limits), + cameras=_gazebo_xacro_cameras(data), + sensors=_gazebo_sensors(data), + ) + + +def render_gazebo_bridge( + data: dict[str, Any], + board_ids: list[str], + urdf_limits: dict[str, tuple[float, float]], + env: jinja2.Environment | None = None, +) -> str: + env = env or _jinja_env() + tpl = env.get_template("gazebo_bridge.yaml.j2") + return tpl.render( + blocks=_ros2_control_blocks(data, board_ids, urdf_limits), + cameras=_gazebo_bridge_cameras(data), + sensors=_gazebo_sensors(data), + ) + + +def render_controllers_yaml( + data: dict[str, Any], + board_ids: list[str], + extra_joints: list[str], + env: jinja2.Environment | None = None, +) -> str: + env = env or _jinja_env() + tpl = env.get_template("controllers.yaml.j2") + controllers: list[dict[str, Any]] = [] + update_rate = int(data["controller_manager"]["update_rate"]) + for bid in board_ids: + ctrl = data["boards"][bid]["controller"] + joints = [ + a["urdf_joint"] for a in _actuators_for_board(data, bid, enabled_only=False) + ] + controllers.append( + { + "name": ctrl["name"], + "type": ctrl["type"], + "joints": joints, + } + ) + return tpl.render( + update_rate=update_rate, + controllers=controllers, + extra_joints=extra_joints, + ) + + +def _resolve_board_ids( + data: dict[str, Any], boards_filter: set[str] | None +) -> list[str]: + board_ids = _board_ids_in_yaml_order(data) + if boards_filter is not None: + board_ids = [b for b in board_ids if b in boards_filter] + if not board_ids: + raise ValueError("no boards left after --boards filter") + return board_ids + + +def generate( + *, + input_yaml: Path, + urdf_xacro: Path, + base_path: Path, + controller_config: Path, + output_dir: Path, + targets: set[str], + boards_filter: set[str] | None, + simulation_only: bool = False, +) -> None: + """Validate YAML and write selected artifacts into output_dir.""" + data = load_hardware_yaml(input_yaml) + validate_hardware_yaml(data) + + if simulation_only: + ros2_control_boards = _board_ids_in_yaml_order(data) + else: + ros2_control_boards = _resolve_board_ids(data, boards_filter) + + output_dir.mkdir(parents=True, exist_ok=True) + names = resolve_generated_files(data) + + urdf_xml = _expand_urdf_xacro(urdf_xacro, base_path, controller_config) + urdf_names, urdf_limits = _parse_urdf_joints_xml(urdf_xml) + + extra: list[str] = [] + if targets & {"controllers", "all"}: + extra = _extra_joints(data, urdf_names) + + env = _jinja_env() + + if targets & {"firmware", "all"} and not simulation_only: + fw_boards = _resolve_board_ids(data, boards_filter) + for bid in fw_boards: + text = render_firmware_c(data, bid, env) + out = output_dir / f"config_{bid}.c" + out.write_text(text, encoding="utf-8") + + if targets & {"ros2_control", "all"}: + xacro_out = output_dir / names["ros2_control_xacro"] + xacro_out.write_text( + render_ros2_control_xacro(data, ros2_control_boards, urdf_limits, env), + encoding="utf-8", + ) + + if targets & {"controllers", "all"}: + yaml_out = output_dir / names["controllers_yaml"] + yaml_out.write_text( + render_controllers_yaml(data, ros2_control_boards, extra, env), + encoding="utf-8", + ) + + if targets & {"gazebo", "all"}: + xacro_out = output_dir / "gazebo.xacro" + bridge_out = output_dir / "gazebo_bridge.yaml" + xacro_out.write_text( + render_gazebo_xacro(data, ros2_control_boards, urdf_limits, env), + encoding="utf-8", + ) + bridge_out.write_text( + render_gazebo_bridge(data, ros2_control_boards, urdf_limits, env), + encoding="utf-8", + ) + + +def generate_from_xacro_string_for_tests( + data: dict[str, Any], + urdf_xml: str, + targets: set[str], + boards_filter: set[str] | None = None, + simulation_only: bool = False, +) -> dict[str, str]: + """In-process generation for unit tests (no xacro binary). Returns name->content.""" + validate_hardware_yaml(data) + if simulation_only: + board_ids = _board_ids_in_yaml_order(data) + firmware_boards = board_ids + else: + board_ids = _resolve_board_ids(data, boards_filter) + firmware_boards = board_ids + urdf_names, urdf_limits = _parse_urdf_joints_xml(urdf_xml) + extra = _extra_joints(data, urdf_names) + names = resolve_generated_files(data) + env = _jinja_env() + out: dict[str, str] = {} + if targets & {"firmware", "all"} and not simulation_only: + for bid in firmware_boards: + out[f"config_{bid}.c"] = render_firmware_c(data, bid, env) + if targets & {"ros2_control", "all"}: + out[names["ros2_control_xacro"]] = render_ros2_control_xacro( + data, board_ids, urdf_limits, env + ) + if targets & {"controllers", "all"}: + out[names["controllers_yaml"]] = render_controllers_yaml( + data, board_ids, extra, env + ) + return out diff --git a/lucy_config_generator/lucy_config_generator/process.py b/lucy_config_generator/lucy_config_generator/process.py new file mode 100644 index 0000000..ccec26a --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/process.py @@ -0,0 +1,90 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""CLI entry point for lucy_config_generator.""" + +from __future__ import annotations + +import argparse +import sys +from pathlib import Path + +from lucy_config_generator.generate import generate + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser( + description=( + "Generate firmware C, ros2_control xacro, and controllers YAML from hardware YAML." + ), + ) + parser.add_argument( + "--input", + type=Path, + required=True, + help="Path to hardware mapping YAML (e.g. urdf/config/hardware/active.yaml).", + ) + parser.add_argument( + "--urdf", + type=Path, + required=True, + help="Top-level robot xacro (e.g. urdf/description/urdf/robot.urdf.xacro).", + ) + parser.add_argument( + "--base-path", + type=Path, + required=True, + help="xacro base_path (installed description/ or source description/).", + ) + parser.add_argument( + "--controller-config", + type=Path, + required=True, + help="controllers.yaml passed into xacro (for extra_joints / URDF processing).", + ) + parser.add_argument( + "--output-dir", + type=Path, + required=True, + help="Directory to write generated files.", + ) + parser.add_argument( + "--targets", + default="all", + choices=("all", "firmware", "ros2_control", "controllers"), + help="Which outputs to generate (default: all).", + ) + parser.add_argument( + "--boards", + default="", + help="Comma-separated board ids to include (default: all known boards in YAML).", + ) + args = parser.parse_args(argv) + + if args.targets == "all": + targets = {"firmware", "ros2_control", "controllers", "gazebo"} + else: + targets = {args.targets} + boards_filter: set[str] | None = None + if args.boards.strip(): + boards_filter = {b.strip() for b in args.boards.split(",") if b.strip()} + + try: + generate( + input_yaml=args.input, + urdf_xacro=args.urdf, + base_path=args.base_path, + controller_config=args.controller_config, + output_dir=args.output_dir, + targets=targets, + boards_filter=boards_filter, + ) + except (OSError, ValueError, RuntimeError, FileNotFoundError) as e: + print(f"error: {e}", file=sys.stderr) + return 1 + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/lucy_config_generator/lucy_config_generator/schema.py b/lucy_config_generator/lucy_config_generator/schema.py new file mode 100644 index 0000000..2e4bad3 --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/schema.py @@ -0,0 +1,546 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""YAML hardware mapping validation.""" + +from __future__ import annotations + +import re +from typing import Any + +REQUIRED_ROOT = ( + "version", + "robot_name", + "firmware", + "controller_manager", + "boards", + "actuators", + "sensors", +) + +# Optional root lists merged for URDF cross-check exclusions (synonyms allowed). +URDF_PASSIVE_LIST_KEYS = ("passive_urdf_joints", "urdf_passive", "urdf_passive_joints") +URDF_IGNORE_LIST_KEYS = ("ignore_urdf_joints", "urdf_ignore", "urdf_ignore_joints") +REQUIRED_ACTUATOR = ( + "id", + "urdf_joint", + "board", + "virtual_pin", + "physical_pin", + "servo_type", + "offset_deg", + "direction", + "scale", + "servo_min_deg", + "servo_max_deg", + "servo_default_deg", + "enabled", +) +REQUIRED_SENSOR = ( + "id", + "type", + "associated_actuator", + "board", + "virtual_pin", + "physical_pin", + "min_value", + "max_value", + "enabled", +) +REQUIRED_BOARD = ( + "serial_id", + "board_class", + "internal_servo_slots", + "firmware_target", + "compile_definition", + "topic_actuators", + "topic_sensors", + "controller", +) +REQUIRED_CAMERA = ( + "name", + "topic", + "message_type", + "external", +) + +# Generated-artifact filenames. Directories are fixed by repo convention +# (``description/ros2_control/`` and ``config/``); only the basename is +# configurable through the optional ``generated_files`` YAML section so the +# generator, config pipeline, URDF include and launches all agree on names. +GENERATED_FILES_KEY = "generated_files" +GENERATED_FILES_DEFAULTS: dict[str, str] = { + "ros2_control_xacro": "inmoov_ros2_control.xacro", + "controllers_yaml": "controllers.yaml", +} + +# Firmware C template: single internal PWM stack vs internal + I2C (PCA) stack. +BOARD_CLASS_INTERNAL_ONLY = "internal_servo_only" +BOARD_CLASS_INTERNAL_I2C_PWM = "internal_servo_i2c_pwm" +BOARD_CLASSES = frozenset({BOARD_CLASS_INTERNAL_ONLY, BOARD_CLASS_INTERNAL_I2C_PWM}) + +_BOARD_ID_RE = re.compile(r"^rp2040_[a-z][a-z0-9_]*$") +_TOPIC_RE = re.compile(r"^[a-z][a-z0-9_/]*$") +_SERIAL_ID_RE = re.compile(r"^[A-Za-z0-9]*$") + + +def ros2_hardware_suffix(board_id: str) -> str: + """Return the snake_case segment after the ``rp2040_`` board id prefix.""" + if not _BOARD_ID_RE.fullmatch(board_id): + raise ValueError( + f"board id {board_id!r} must match {_BOARD_ID_RE.pattern} " + "(rp2040_ prefix + snake_case suffix)" + ) + return board_id[7:] # len("rp2040_") == 7 + + +def derive_ros2_hardware_name(board_id: str) -> str: + """ + Return ```` for *board_id*. + + Pattern: ``LucyHardware`` + PascalCase of the segment after ``rp2040_``. + Example: ``rp2040_left_arm`` → ``LucyHardwareLeftArm``. + """ + suffix = ros2_hardware_suffix(board_id) + parts = [p for p in suffix.split("_") if p] + if not parts: + raise ValueError(f"board id {board_id!r}: empty suffix after rp2040_") + inner = "".join(p[:1].upper() + p[1:].lower() if p else "" for p in parts) + return "LucyHardware" + inner + + +def derive_ros2_node_name(board_id: str) -> str: + """ + Return the ``node_name`` hardware parameter for *board_id*. + + Pattern: ``lucy_hardware_interface_`` + snake_case suffix after ``rp2040_``. + """ + return "lucy_hardware_interface_" + ros2_hardware_suffix(board_id) + + +def resolve_generated_files(data: dict[str, Any]) -> dict[str, str]: + """ + Return generated-artifact filenames, applying defaults for missing keys. + + The optional ``generated_files`` mapping carries bare filenames (no path + separators); the on-disk directories are fixed by repo convention. Unknown + keys are ignored so the section can grow without breaking older configs. + """ + section = data.get(GENERATED_FILES_KEY) + if section is None: + return dict(GENERATED_FILES_DEFAULTS) + if not isinstance(section, dict): + raise ValueError(f"{GENERATED_FILES_KEY} must be a mapping") + out = dict(GENERATED_FILES_DEFAULTS) + for key in GENERATED_FILES_DEFAULTS: + if key not in section: + continue + value = section[key] + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{GENERATED_FILES_KEY}.{key} must be a non-empty string") + value = value.strip() + if "/" in value or "\\" in value or value in (".", ".."): + raise ValueError( + f"{GENERATED_FILES_KEY}.{key} must be a bare filename " + "(no path separators); directories are fixed by convention" + ) + out[key] = value + return out + + +def _label(entity: dict[str, Any]) -> str: + """Return entity id label used in validation errors.""" + value = entity.get("id") + if isinstance(value, str) and value.strip(): + return value + return "" + + +def _append_missing_keys( + errors: list[str], prefix: str, item: dict[str, Any], required: tuple[str, ...] +) -> bool: + """Append missing-key errors and return False if any are missing.""" + ok = True + for key in required: + if key not in item: + errors.append(f"{prefix}: missing {key}") + ok = False + return ok + + +def _validate_boards(boards: dict[str, Any], errors: list[str]) -> None: + """Validate board definitions and append all discovered errors.""" + for board_id, board in boards.items(): + if not _append_missing_keys(errors, f"board {board_id}", board, REQUIRED_BOARD): + continue + + controller = board["controller"] + if not isinstance(controller, dict): + errors.append(f"board {board_id}: controller must be a mapping") + continue + if "name" not in controller or "type" not in controller: + errors.append(f"board {board_id}: controller needs name and type") + else: + for key in ("name", "type"): + value = controller[key] + if not isinstance(value, str) or not value.strip(): + errors.append(f"board {board_id}: controller.{key} must be a non-empty string") + + board_class = board["board_class"] + if board_class not in BOARD_CLASSES: + errors.append( + f"board {board_id}: board_class must be one of {sorted(BOARD_CLASSES)}, " + f"got {board_class!r}" + ) + + for key in ("firmware_target", "compile_definition"): + value = board[key] + if not isinstance(value, str) or not value.strip(): + errors.append(f"board {board_id}: {key} must be a non-empty string") + + serial_id = board["serial_id"] + if not isinstance(serial_id, str): + errors.append(f"board {board_id}: serial_id must be a string") + elif serial_id and not _SERIAL_ID_RE.fullmatch(serial_id): + errors.append( + f"board {board_id}: serial_id must be empty or alphanumeric " + "(USB serial / picotool --ser)" + ) + + try: + slots = int(board["internal_servo_slots"]) + except Exception: + errors.append(f"board {board_id}: internal_servo_slots must be an integer") + slots = 0 + if slots < 1: + errors.append(f"board {board_id}: internal_servo_slots must be >= 1") + + for key in ("topic_actuators", "topic_sensors"): + topic = board[key] + if not isinstance(topic, str) or not topic: + errors.append(f"board {board_id}: {key} must be a non-empty string") + elif not _TOPIC_RE.fullmatch(topic): + errors.append( + f"board {board_id}: {key} must match {_TOPIC_RE.pattern} (no leading slash)" + ) + + try: + derive_ros2_hardware_name(board_id) + derive_ros2_node_name(board_id) + except ValueError as exc: + errors.append(f"board {board_id}: {exc}") + + if board["topic_actuators"] != board["topic_actuators"].strip(): + errors.append( + f"board {board_id}: topic_actuators must not have surrounding whitespace" + ) + + +def _validate_actuator( + actuator: dict[str, Any], + boards: dict[str, Any], + errors: list[str], + by_board: dict[str, list[dict[str, Any]]], + has_item_errors_by_board: dict[str, bool], +) -> None: + """Validate one actuator and append it to board bucket when valid.""" + aid = _label(actuator) + if not _append_missing_keys(errors, f"actuator {aid}", actuator, REQUIRED_ACTUATOR): + return + + valid = True + for key in ("id", "urdf_joint", "board"): + if not isinstance(actuator[key], str) or not actuator[key].strip(): + errors.append(f"actuator {aid}: {key} must be a non-empty string") + valid = False + + if not isinstance(actuator["enabled"], bool): + errors.append(f"actuator {aid}: enabled must be a boolean") + valid = False + + board_id = actuator["board"] + if board_id not in boards: + errors.append(f"actuator {aid}: unknown board {board_id}") + valid = False + + try: + int(actuator["virtual_pin"]) + except Exception: + errors.append(f"actuator {aid}: virtual_pin must be an integer") + valid = False + + try: + pin = int(actuator["physical_pin"]) + except Exception: + errors.append(f"actuator {aid}: physical_pin must be an integer") + valid = False + pin = 0 + + board_slots = 0 + if board_id in boards: + try: + board_slots = int(boards[board_id]["internal_servo_slots"]) + except Exception: + errors.append( + f"actuator {aid}: cannot validate physical_pin because " + f"board {board_id} internal_servo_slots is invalid" + ) + valid = False + if valid and (pin < 1 or pin > board_slots): + errors.append( + f"actuator {aid}: physical_pin {pin} out of range 1..{board_slots}" + ) + valid = False + + servo_type = str(actuator["servo_type"]).strip('"') + if servo_type not in ("180", "270", "300"): + errors.append(f"actuator {aid}: invalid servo_type {servo_type!r}") + valid = False + + try: + float(actuator["offset_deg"]) + except Exception: + errors.append(f"actuator {aid}: offset_deg must be numeric") + valid = False + + try: + direction = float(actuator["direction"]) + except Exception: + errors.append(f"actuator {aid}: direction must be numeric") + valid = False + direction = 0.0 + if valid and direction not in (-1.0, 1.0): + errors.append(f"actuator {aid}: direction must be -1 or 1") + valid = False + + try: + scale = float(actuator["scale"]) + except Exception: + errors.append(f"actuator {aid}: scale must be numeric") + valid = False + scale = 1.0 + if valid and scale == 0.0: + errors.append(f"actuator {aid}: scale must be non-zero") + valid = False + + if valid: + by_board.setdefault(board_id, []).append(actuator) + elif board_id in boards: + has_item_errors_by_board[board_id] = True + + +def _validate_actuator_ranges( + by_board: dict[str, list[dict[str, Any]]], + has_item_errors_by_board: dict[str, bool], + errors: list[str], +) -> None: + """Validate virtual pin contiguity and servo ranges for each board.""" + for board_id, actuators in by_board.items(): + if has_item_errors_by_board.get(board_id, False): + continue + virtual_pins = sorted(int(act["virtual_pin"]) for act in actuators) + if len(virtual_pins) != len(set(virtual_pins)): + errors.append(f"board {board_id}: duplicate virtual_pin") + continue + if virtual_pins: + max_pin = virtual_pins[-1] + expected = set(range(max_pin + 1)) + missing = sorted(expected - set(virtual_pins)) + if missing: + errors.append( + f"board {board_id}: missing virtual_pin indices {missing} " + "(no actuator mapped to these virtual_pin values)" + ) + if virtual_pins != list(range(len(virtual_pins))): + errors.append( + f"board {board_id}: virtual_pin must be contiguous from 0..N-1, " + f"got {virtual_pins}" + ) + + for actuator in actuators: + aid = _label(actuator) + lo = actuator["servo_min_deg"] + hi = actuator["servo_max_deg"] + default = actuator["servo_default_deg"] + try: + lo_f = float(lo) + hi_f = float(hi) + default_f = float(default) + except Exception: + errors.append( + f"actuator {aid}: servo_min_deg/servo_max_deg/" + "servo_default_deg must be numeric" + ) + continue + + if lo_f > hi_f: + errors.append( + f"actuator {aid}: servo_min_deg {lo} must be <= servo_max_deg {hi}" + ) + if default_f < lo_f or default_f > hi_f: + errors.append(f"actuator {aid}: servo_default_deg out of [{lo}, {hi}]") + + +def _validate_sensor( + sensor: dict[str, Any], + boards: dict[str, Any], + actuator_ids: set[str], + errors: list[str], + by_board: dict[str, list[dict[str, Any]]], + has_item_errors_by_board: dict[str, bool], +) -> None: + """Validate one sensor and append it to board bucket when valid.""" + sid = _label(sensor) + if not _append_missing_keys(errors, f"sensor {sid}", sensor, REQUIRED_SENSOR): + return + + valid = True + for key in ("id", "type", "associated_actuator", "board"): + if not isinstance(sensor[key], str) or not sensor[key].strip(): + errors.append(f"sensor {sid}: {key} must be a non-empty string") + valid = False + + if not isinstance(sensor["enabled"], bool): + errors.append(f"sensor {sid}: enabled must be a boolean") + valid = False + + board_id = sensor["board"] + if board_id not in boards: + errors.append(f"sensor {sid}: unknown board {board_id}") + valid = False + + associated_actuator = sensor["associated_actuator"] + if associated_actuator not in actuator_ids: + errors.append(f"sensor {sid}: associated_actuator {associated_actuator} not found") + valid = False + + try: + int(sensor["virtual_pin"]) + except Exception: + errors.append(f"sensor {sid}: virtual_pin must be an integer") + valid = False + + min_value = sensor["min_value"] + max_value = sensor["max_value"] + min_num: float | None = None + max_num: float | None = None + + if min_value is not None: + try: + min_num = float(min_value) + except Exception: + errors.append(f"sensor {sid}: min_value must be numeric or null") + valid = False + + if max_value is not None: + try: + max_num = float(max_value) + except Exception: + errors.append(f"sensor {sid}: max_value must be numeric or null") + valid = False + + if valid and min_num is not None and max_num is not None and min_num > max_num: + errors.append(f"sensor {sid}: min_value {min_value} must be <= max_value {max_value}") + valid = False + + if valid: + by_board.setdefault(board_id, []).append(sensor) + elif board_id in boards: + has_item_errors_by_board[board_id] = True + + +def _validate_sensor_ranges( + by_board: dict[str, list[dict[str, Any]]], + has_item_errors_by_board: dict[str, bool], + errors: list[str], +) -> None: + """Validate sensor virtual-pin contiguity for each board.""" + for board_id, sensors in by_board.items(): + if has_item_errors_by_board.get(board_id, False): + continue + virtual_pins = sorted(int(sensor["virtual_pin"]) for sensor in sensors) + if len(virtual_pins) != len(set(virtual_pins)): + errors.append(f"board {board_id}: duplicate sensor virtual_pin") + continue + if virtual_pins: + max_pin = virtual_pins[-1] + expected = set(range(max_pin + 1)) + missing = sorted(expected - set(virtual_pins)) + if missing: + errors.append( + f"board {board_id}: missing sensor virtual_pin indices {missing} " + "(no sensor mapped to these virtual_pin values)" + ) + if virtual_pins != list(range(len(virtual_pins))): + errors.append( + f"board {board_id}: sensor virtual_pin must be contiguous 0..N-1, " + f"got {virtual_pins}" + ) + + +def validate_hardware_yaml(data: dict[str, Any]) -> None: + """Raise ValueError if the mapping is structurally invalid.""" + for key in REQUIRED_ROOT: + if key not in data: + raise ValueError(f"missing root key: {key}") + if data["version"] != 1: + raise ValueError("version must be 1") + + resolve_generated_files(data) + + for passive_key in URDF_PASSIVE_LIST_KEYS + URDF_IGNORE_LIST_KEYS: + plist = data.get(passive_key) + if plist is None: + continue + if not isinstance(plist, list): + raise ValueError(f"{passive_key} must be a list of strings") + for i, item in enumerate(plist): + if not isinstance(item, str) or not item.strip(): + raise ValueError(f"{passive_key}[{i}] must be a non-empty string") + + boards: dict[str, Any] = data["boards"] + if not isinstance(boards, dict) or not boards: + raise ValueError("boards must be a non-empty map") + + errors: list[str] = [] + _validate_boards(boards, errors) + + actuators: list[dict[str, Any]] = data["actuators"] + if not isinstance(actuators, list): + raise ValueError("actuators must be a list") + actuators_by_board: dict[str, list[dict[str, Any]]] = {} + actuator_item_errors_by_board: dict[str, bool] = {} + for actuator in actuators: + _validate_actuator( + actuator, + boards, + errors, + actuators_by_board, + actuator_item_errors_by_board, + ) + _validate_actuator_ranges(actuators_by_board, actuator_item_errors_by_board, errors) + + sensors: list[dict[str, Any]] = data["sensors"] + if not isinstance(sensors, list): + raise ValueError("sensors must be a list") + actuator_ids = { + actuator.get("id") + for actuator in actuators + if isinstance(actuator.get("id"), str) and actuator.get("id", "").strip() + } + sensors_by_board: dict[str, list[dict[str, Any]]] = {} + sensor_item_errors_by_board: dict[str, bool] = {} + for sensor in sensors: + _validate_sensor( + sensor, + boards, + actuator_ids, + errors, + sensors_by_board, + sensor_item_errors_by_board, + ) + _validate_sensor_ranges(sensors_by_board, sensor_item_errors_by_board, errors) + + if errors: + raise ValueError("\n".join(errors)) diff --git a/lucy_config_generator/lucy_config_generator/templates/config_internal_i2c_board.c.j2 b/lucy_config_generator/lucy_config_generator/templates/config_internal_i2c_board.c.j2 new file mode 100644 index 0000000..5c984db --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/config_internal_i2c_board.c.j2 @@ -0,0 +1,39 @@ +/* Generated by lucy_config_generator — do not edit. */ +#include "constant.h" +#include "joint.h" +#include "board.h" + +const board_t boardtmp = {2}; + +void dump_config(void) { + memset(boards, 0, sizeof(boards)); + + board_t *internal = &boards[0]; + internal->init = &internal_init; + internal->on_joint_subscriber_update = &internal_on_joint_subscriber_update; + internal->on_sensor_timer_tick = &internal_on_sensor_timer_tick; + + board_t *adafruit = ++internal; + adafruit->pin.i2c.sda = INTERNAL_I2C_SDA; + adafruit->pin.i2c.scl = INTERNAL_I2C_SCL; + adafruit->pin.i2c.port = INTERNAL_I2C_PORT; + adafruit->init = &adafruit_init; + adafruit->on_joint_subscriber_update = &adafruit_on_joint_subscriber_update; + + joint_t *joint = internal->data.internal.joints; + +{% for a in actuators %} + /* {{ a.id }} */ + joint->config.virtual_pin = {{ a.virtual_pin }}; + joint->config.physical_pin = INTERNAL_SERVO_{{ a.physical_pin }}; + joint->config.servo_type = SERVO_TYPE_{{ a.servo_type | replace('"', '') }}; + joint->config.security_min_angle = DEG_TO_RAD({{ a.servo_min_deg }}); + joint->config.security_max_angle = DEG_TO_RAD({{ a.servo_max_deg }}); + joint->config.default_angle = DEG_TO_RAD({{ a.servo_default_deg }}); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; +{% endfor %} + + joint = adafruit->data.adafruit.joints; +} diff --git a/lucy_config_generator/lucy_config_generator/templates/config_internal_only_board.c.j2 b/lucy_config_generator/lucy_config_generator/templates/config_internal_only_board.c.j2 new file mode 100644 index 0000000..55e929e --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/config_internal_only_board.c.j2 @@ -0,0 +1,27 @@ +/* Generated by lucy_config_generator — do not edit. */ +#include "constant.h" +#include "joint.h" +#include "board.h" + +void dump_config(void) { + memset(boards, 0, sizeof(boards)); + + board_t *board = &boards[0]; + board->init = &internal_init; + board->on_joint_subscriber_update = &internal_on_joint_subscriber_update; + + joint_t *joint = board->data.internal.joints; + +{% for a in actuators %} + /* {{ a.id }} */ + joint->config.virtual_pin = {{ a.virtual_pin }}; + joint->config.physical_pin = INTERNAL_SERVO_{{ a.physical_pin }}; + joint->config.servo_type = SERVO_TYPE_{{ a.servo_type | replace('"', '') }}; + joint->config.security_min_angle = DEG_TO_RAD({{ a.servo_min_deg }}); + joint->config.security_max_angle = DEG_TO_RAD({{ a.servo_max_deg }}); + joint->config.default_angle = DEG_TO_RAD({{ a.servo_default_deg }}); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; +{% endfor %} +} diff --git a/lucy_config_generator/lucy_config_generator/templates/controllers.yaml.j2 b/lucy_config_generator/lucy_config_generator/templates/controllers.yaml.j2 new file mode 100644 index 0000000..8701f73 --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/controllers.yaml.j2 @@ -0,0 +1,32 @@ +# Generated by lucy_config_generator — do not edit. +controller_manager: + ros__parameters: + update_rate: {{ update_rate }} + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster +{% for c in controllers %} + {{ c.name }}: + type: {{ c.type }} +{% endfor %} + +joint_state_broadcaster: + ros__parameters: + extra_joints: +{% for name in extra_joints %} + - {{ name }} +{% endfor %} + +{% for c in controllers %} +{{ c.name }}: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: +{% for j in c.joints %} + - {{ j }} +{% endfor %} + command_interfaces: + - position + state_interfaces: + - position + +{% endfor %} diff --git a/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 b/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 new file mode 100644 index 0000000..a8c32d8 --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/gazebo.xacro.j2 @@ -0,0 +1,70 @@ + + + + +{% for block in blocks %} + + + gz_ros2_control/GazeboSimSystem + +{% for j in block.joints %} + + {{ j.virtual_pin }} + {{ j.servo_type | replace('"', '') }} + {{ j.offset_deg }} + {{ j.direction }} + {{ j.scale }} + {{ j.servo_min_deg }} + {{ j.servo_max_deg }} + {{ j.servo_default_deg }} + + + +{% endfor %} + +{% endfor %} + +{% for sensor in sensors %} + + + 10 + {{ sensor.topic }} +{% for s in sensor.sensors %} + +{% endfor %} + + +{% endfor %} + +{% for camera in cameras %} + + + true + 30.0 + true + + {{ camera.topic }} + + 1.047 + + 640 + 480 + R8G8B8 + + + 0.1 + 100.0 + + + + +{% endfor %} + + + + $(arg controller_config) + + + + + diff --git a/lucy_config_generator/lucy_config_generator/templates/gazebo_bridge.yaml.j2 b/lucy_config_generator/lucy_config_generator/templates/gazebo_bridge.yaml.j2 new file mode 100644 index 0000000..8ea864f --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/gazebo_bridge.yaml.j2 @@ -0,0 +1,21 @@ +# Generated by lucy_config_generator — do not edit. +- ros_topic_name: "/clock" + gz_topic_name: "/world/default/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS +{% for camera in cameras %} +# Raw frames; gazebo.launch.py republishes {{ camera.raw_topic }} -> {{ camera.topic }} (CompressedImage) for the LCP. +- ros_topic_name: "{{ camera.topic }}" + gz_topic_name: "{{ camera.gz_topic }}" + ros_type_name: "sensor_msgs/msg/Image" + gz_type_name: "gz.msgs.Image" + direction: GZ_TO_ROS +{% endfor %} +{% for sensor in sensors %} +- ros_topic_name: "{{ sensor.topic }}" + gz_topic_name: "{{ sensor.topic }}" + ros_type_name: "ros_gz_interfaces/msg/Float32Array" + gz_type_name: "gz.msgs.Float_V" + direction: GZ_TO_ROS +{% endfor %} diff --git a/lucy_config_generator/lucy_config_generator/templates/ros2_control.xacro.j2 b/lucy_config_generator/lucy_config_generator/templates/ros2_control.xacro.j2 new file mode 100644 index 0000000..7147c27 --- /dev/null +++ b/lucy_config_generator/lucy_config_generator/templates/ros2_control.xacro.j2 @@ -0,0 +1,41 @@ + + + + + +{% for block in blocks %} + + + lucy_ros2_control/LucySystemHardware + + false + + + {{ block.publisher_topic }} + {{ block.node_name }} + + +{% for j in block.joints %} + + {{ j.virtual_pin }} + {{ j.servo_type | replace('"', '') }} + {{ j.offset_deg }} + {{ j.direction }} + {{ j.scale }} + {{ j.servo_min_deg }} + {{ j.servo_max_deg }} + {{ j.servo_default_deg }} + +{% if j.limit_lower_rad is defined and j.limit_upper_rad is defined %} + {{ j.limit_lower_rad }} + {{ j.limit_upper_rad }} +{% endif %} + + + +{% endfor %} + +{% endfor %} + + + diff --git a/lucy_config_generator/package.xml b/lucy_config_generator/package.xml new file mode 100644 index 0000000..23d2603 --- /dev/null +++ b/lucy_config_generator/package.xml @@ -0,0 +1,22 @@ + + + + lucy_config_generator + 0.0.0 + Generate RP2040 firmware C, ros2_control xacro, and controllers.yaml from hardware YAML (#96). + Sentience Robotics Team + GPL-3.0 + + python3-jinja2 + python3-yaml + xacro + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/lucy_config_generator/requirements.txt b/lucy_config_generator/requirements.txt new file mode 100644 index 0000000..33ba4e9 --- /dev/null +++ b/lucy_config_generator/requirements.txt @@ -0,0 +1,2 @@ +Jinja2 +PyYAML diff --git a/lucy_config_generator/resource/lucy_config_generator b/lucy_config_generator/resource/lucy_config_generator new file mode 100644 index 0000000..e69de29 diff --git a/lucy_config_generator/setup.cfg b/lucy_config_generator/setup.cfg new file mode 100644 index 0000000..94c74e1 --- /dev/null +++ b/lucy_config_generator/setup.cfg @@ -0,0 +1,11 @@ +[develop] +script_dir=$base/lib/lucy_config_generator + +[install] +install_scripts=$base/lib/lucy_config_generator + +[flake8] +max-line-length = 100 + +[tool:pytest] +testpaths = test diff --git a/lucy_config_generator/setup.py b/lucy_config_generator/setup.py new file mode 100644 index 0000000..846bbc9 --- /dev/null +++ b/lucy_config_generator/setup.py @@ -0,0 +1,31 @@ +from setuptools import find_packages, setup + +package_name = "lucy_config_generator" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + package_data={package_name: ["templates/*.j2"]}, + include_package_data=True, + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools", "Jinja2", "PyYAML"], + zip_safe=True, + maintainer="Sentience Robotics Team", + maintainer_email="contact@sentience-robotics.fr", + description="Generate RP2040 and ros2_control artifacts from thais_urdf hardware YAML (#96).", + license="GPL-3.0", + extras_require={ + "test": [ + "pytest", + ], + }, + entry_points={ + "console_scripts": [ + "generate_config = lucy_config_generator.process:main", + ], + }, +) diff --git a/lucy_config_generator/test/fixtures/golden_config_rp2040_left_arm.c b/lucy_config_generator/test/fixtures/golden_config_rp2040_left_arm.c new file mode 100644 index 0000000..e430bed --- /dev/null +++ b/lucy_config_generator/test/fixtures/golden_config_rp2040_left_arm.c @@ -0,0 +1,25 @@ +/* Generated by lucy_config_generator — do not edit. */ +#include "constant.h" +#include "joint.h" +#include "board.h" + +void dump_config(void) { + memset(boards, 0, sizeof(boards)); + + board_t *board = &boards[0]; + board->init = &internal_init; + board->on_joint_subscriber_update = &internal_on_joint_subscriber_update; + + joint_t *joint = board->data.internal.joints; + + /* left_a */ + joint->config.virtual_pin = 0; + joint->config.physical_pin = INTERNAL_SERVO_10; + joint->config.servo_type = SERVO_TYPE_270; + joint->config.security_min_angle = DEG_TO_RAD(0.0); + joint->config.security_max_angle = DEG_TO_RAD(270.0); + joint->config.default_angle = DEG_TO_RAD(135.0); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; +} \ No newline at end of file diff --git a/lucy_config_generator/test/fixtures/golden_config_rp2040_right_arm.c b/lucy_config_generator/test/fixtures/golden_config_rp2040_right_arm.c new file mode 100644 index 0000000..30ff312 --- /dev/null +++ b/lucy_config_generator/test/fixtures/golden_config_rp2040_right_arm.c @@ -0,0 +1,25 @@ +/* Generated by lucy_config_generator — do not edit. */ +#include "constant.h" +#include "joint.h" +#include "board.h" + +void dump_config(void) { + memset(boards, 0, sizeof(boards)); + + board_t *board = &boards[0]; + board->init = &internal_init; + board->on_joint_subscriber_update = &internal_on_joint_subscriber_update; + + joint_t *joint = board->data.internal.joints; + + /* right_a */ + joint->config.virtual_pin = 0; + joint->config.physical_pin = INTERNAL_SERVO_10; + joint->config.servo_type = SERVO_TYPE_270; + joint->config.security_min_angle = DEG_TO_RAD(0.0); + joint->config.security_max_angle = DEG_TO_RAD(270.0); + joint->config.default_angle = DEG_TO_RAD(135.0); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; +} \ No newline at end of file diff --git a/lucy_config_generator/test/fixtures/golden_config_rp2040_torso_head.c b/lucy_config_generator/test/fixtures/golden_config_rp2040_torso_head.c new file mode 100644 index 0000000..f575ab0 --- /dev/null +++ b/lucy_config_generator/test/fixtures/golden_config_rp2040_torso_head.c @@ -0,0 +1,47 @@ +/* Generated by lucy_config_generator — do not edit. */ +#include "constant.h" +#include "joint.h" +#include "board.h" + +const board_t boardtmp = {2}; + +void dump_config(void) { + memset(boards, 0, sizeof(boards)); + + board_t *internal = &boards[0]; + internal->init = &internal_init; + internal->on_joint_subscriber_update = &internal_on_joint_subscriber_update; + internal->on_sensor_timer_tick = &internal_on_sensor_timer_tick; + + board_t *adafruit = ++internal; + adafruit->pin.i2c.sda = INTERNAL_I2C_SDA; + adafruit->pin.i2c.scl = INTERNAL_I2C_SCL; + adafruit->pin.i2c.port = INTERNAL_I2C_PORT; + adafruit->init = &adafruit_init; + adafruit->on_joint_subscriber_update = &adafruit_on_joint_subscriber_update; + + joint_t *joint = internal->data.internal.joints; + + /* torso_left_y */ + joint->config.virtual_pin = 0; + joint->config.physical_pin = INTERNAL_SERVO_5; + joint->config.servo_type = SERVO_TYPE_270; + joint->config.security_min_angle = DEG_TO_RAD(0.0); + joint->config.security_max_angle = DEG_TO_RAD(270.0); + joint->config.default_angle = DEG_TO_RAD(0.0); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; + /* torso_right_y */ + joint->config.virtual_pin = 1; + joint->config.physical_pin = INTERNAL_SERVO_6; + joint->config.servo_type = SERVO_TYPE_270; + joint->config.security_min_angle = DEG_TO_RAD(0.0); + joint->config.security_max_angle = DEG_TO_RAD(270.0); + joint->config.default_angle = DEG_TO_RAD(0.0); + joint->config.init_joint = &internal_joint_init; + joint->config.move_joint = &internal_joint_move; + joint++; + + joint = adafruit->data.adafruit.joints; +} \ No newline at end of file diff --git a/lucy_config_generator/test/fixtures/golden_controllers.yaml b/lucy_config_generator/test/fixtures/golden_controllers.yaml new file mode 100644 index 0000000..ec72eec --- /dev/null +++ b/lucy_config_generator/test/fixtures/golden_controllers.yaml @@ -0,0 +1,50 @@ +# Generated by lucy_config_generator — do not edit. +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + torso_head_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_state_broadcaster: + ros__parameters: + extra_joints: + - passive_extra_joint + +left_arm_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - left_a_joint + - left_disabled_joint + command_interfaces: + - position + state_interfaces: + - position + +right_arm_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - right_a_joint + command_interfaces: + - position + state_interfaces: + - position + +torso_head_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - left_shoulder_y_link_joint + - right_shoulder_y_link_joint + command_interfaces: + - position + state_interfaces: + - position + diff --git a/lucy_config_generator/test/fixtures/golden_inmoov_ros2_control.xacro b/lucy_config_generator/test/fixtures/golden_inmoov_ros2_control.xacro new file mode 100644 index 0000000..f6abc61 --- /dev/null +++ b/lucy_config_generator/test/fixtures/golden_inmoov_ros2_control.xacro @@ -0,0 +1,118 @@ + + + + + + + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/left_arm + lucy_hardware_interface_left_arm + + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + 0.0 + 1.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + 0.0 + 1.0 + + + + + + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/right_arm + lucy_hardware_interface_right_arm + + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + 0.0 + 1.0 + + + + + + + lucy_ros2_control/LucySystemHardware + + false + + + actuators/torso + lucy_hardware_interface_torso_head + + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 + + 0.0 + 1.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 + + 0.0 + 1.0 + + + + + + diff --git a/lucy_config_generator/test/fixtures/test_mapping.yaml b/lucy_config_generator/test/fixtures/test_mapping.yaml new file mode 100644 index 0000000..5155fe5 --- /dev/null +++ b/lucy_config_generator/test/fixtures/test_mapping.yaml @@ -0,0 +1,123 @@ +# Minimal hardware mapping for generator golden tests (#96). +version: 1 +robot_name: testbot + +firmware: + source_dir: firmware_stub + build_dir: build + +controller_manager: + update_rate: 100 + +boards: + rp2040_left_arm: + serial_id: "" + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_left_arm + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: + name: left_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_right_arm: + serial_id: "" + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_right_arm + compile_definition: USE_RIGHT_ARM + topic_actuators: actuators/right_arm + topic_sensors: sensors/right_arm + controller: + name: right_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_torso_head: + serial_id: "" + board_class: internal_servo_i2c_pwm + internal_servo_slots: 18 + firmware_target: pico_micro_ros_torso + compile_definition: USE_TORSO + topic_actuators: actuators/torso + topic_sensors: sensors/torso + controller: + name: torso_head_controller + type: joint_trajectory_controller/JointTrajectoryController + +actuators: + - id: left_a + urdf_joint: left_a_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: true + - id: left_disabled + urdf_joint: left_disabled_joint + board: rp2040_left_arm + virtual_pin: 1 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + - id: right_a + urdf_joint: right_a_joint + board: rp2040_right_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: true + - id: torso_left_y + urdf_joint: left_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 0 + physical_pin: 5 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: true + - id: torso_right_y + urdf_joint: right_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 1 + physical_pin: 6 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: true + +sensors: + - id: left_thumb_pressure + type: pressure + associated_actuator: left_a + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 16 + min_value: null + max_value: null + enabled: true diff --git a/lucy_config_generator/test/fixtures/test_robot.urdf.xacro b/lucy_config_generator/test/fixtures/test_robot.urdf.xacro new file mode 100644 index 0000000..9e57b39 --- /dev/null +++ b/lucy_config_generator/test/fixtures/test_robot.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lucy_config_generator/test/test_copyright.py b/lucy_config_generator/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/lucy_config_generator/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/lucy_config_generator/test/test_flake8.py b/lucy_config_generator/test/test_flake8.py new file mode 100644 index 0000000..006c468 --- /dev/null +++ b/lucy_config_generator/test/test_flake8.py @@ -0,0 +1,41 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import warnings +from pathlib import Path + +import pytest +from ament_flake8.main import main_with_errors + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + pkg_root = Path(__file__).resolve().parents[1] + targets = [ + str(pkg_root / "setup.py"), + str(pkg_root / "lucy_config_generator"), + str(pkg_root / "test"), + ] + # flake8 uses importlib.metadata.entry_points().get(); Python 3.10+ deprecates it. + with warnings.catch_warnings(): + warnings.filterwarnings( + "ignore", + message="SelectableGroups dict interface is deprecated", + category=DeprecationWarning, + ) + rc, errors = main_with_errors(argv=targets) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/lucy_config_generator/test/test_generate.py b/lucy_config_generator/test/test_generate.py new file mode 100644 index 0000000..513fd3d --- /dev/null +++ b/lucy_config_generator/test/test_generate.py @@ -0,0 +1,277 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Golden tests for lucy_config_generator (#96).""" + +from __future__ import annotations + +from pathlib import Path + +import pytest +import yaml + +from lucy_config_generator.generate import generate_from_xacro_string_for_tests +from lucy_config_generator.schema import ( + GENERATED_FILES_DEFAULTS, + derive_ros2_hardware_name, + derive_ros2_node_name, + resolve_generated_files, + validate_hardware_yaml, +) + +_FIXTURES = Path(__file__).resolve().parent / "fixtures" + + +def _fixture_urdf_xml() -> str: + """Return mock robot XML from test_robot.urdf.xacro for joint-name extraction.""" + return (_FIXTURES / "test_robot.urdf.xacro").read_text(encoding="utf-8") + + +def _load_mapping() -> dict: + with (_FIXTURES / "test_mapping.yaml").open(encoding="utf-8") as f: + return yaml.safe_load(f) + + +def test_schema_accepts_fixture(): + validate_hardware_yaml(_load_mapping()) + + +def test_ros2_names_derived_from_board_id(): + assert derive_ros2_hardware_name("rp2040_left_arm") == "LucyHardwareLeftArm" + assert derive_ros2_node_name("rp2040_left_arm") == "lucy_hardware_interface_left_arm" + assert derive_ros2_hardware_name("rp2040_torso_head") == "LucyHardwareTorsoHead" + assert derive_ros2_node_name("rp2040_torso_head") == "lucy_hardware_interface_torso_head" + + +def test_schema_rejects_bad_version(): + data = _load_mapping() + data["version"] = 2 + with pytest.raises(ValueError, match="version"): + validate_hardware_yaml(data) + + +def test_schema_rejects_empty_compile_definition(): + data = _load_mapping() + data["boards"]["rp2040_left_arm"]["compile_definition"] = "" + with pytest.raises(ValueError, match="compile_definition"): + validate_hardware_yaml(data) + + +def test_schema_rejects_empty_controller_name(): + data = _load_mapping() + data["boards"]["rp2040_left_arm"]["controller"]["name"] = "" + with pytest.raises(ValueError, match=r"controller\.name"): + validate_hardware_yaml(data) + + +def test_schema_rejects_invalid_internal_servo_slots(): + data = _load_mapping() + data["boards"]["rp2040_left_arm"]["internal_servo_slots"] = 0 + with pytest.raises(ValueError, match="internal_servo_slots must be >= 1"): + validate_hardware_yaml(data) + + +def test_schema_rejects_null_virtual_pin(): + data = _load_mapping() + data["actuators"][0]["virtual_pin"] = None + with pytest.raises(ValueError, match="virtual_pin must be an integer"): + validate_hardware_yaml(data) + + +def test_schema_rejects_servo_min_greater_than_max(): + data = _load_mapping() + data["actuators"][0]["servo_min_deg"] = 10 + data["actuators"][0]["servo_max_deg"] = 5 + data["actuators"][0]["servo_default_deg"] = 7 + with pytest.raises(ValueError, match="servo_min_deg .* must be <= servo_max_deg"): + validate_hardware_yaml(data) + + +def test_schema_rejects_physical_pin_above_board_slot_limit(): + data = _load_mapping() + data["boards"]["rp2040_left_arm"]["internal_servo_slots"] = 8 + data["actuators"][0]["physical_pin"] = 9 + with pytest.raises(ValueError, match=r"physical_pin 9 out of range 1..8"): + validate_hardware_yaml(data) + + +def test_schema_rejects_non_bool_enabled_actuator(): + data = _load_mapping() + data["actuators"][0]["enabled"] = "fal" + with pytest.raises(ValueError, match="enabled must be a boolean"): + validate_hardware_yaml(data) + + +def test_schema_rejects_non_bool_enabled_sensor(): + data = _load_mapping() + data["sensors"][0]["enabled"] = "fal" + with pytest.raises(ValueError, match="enabled must be a boolean"): + validate_hardware_yaml(data) + + +def test_schema_rejects_non_numeric_offset_deg(): + data = _load_mapping() + data["actuators"][0]["offset_deg"] = None + with pytest.raises(ValueError, match="offset_deg must be numeric"): + validate_hardware_yaml(data) + + +def test_schema_rejects_non_unit_direction(): + data = _load_mapping() + data["actuators"][0]["direction"] = 0 + with pytest.raises(ValueError, match="direction must be -1 or 1"): + validate_hardware_yaml(data) + + +def test_schema_rejects_zero_scale(): + data = _load_mapping() + data["actuators"][0]["scale"] = 0 + with pytest.raises(ValueError, match="scale must be non-zero"): + validate_hardware_yaml(data) + + +def test_schema_rejects_empty_sensor_id(): + data = _load_mapping() + data["sensors"][0]["id"] = "" + with pytest.raises(ValueError, match=r"sensor .*: id must be a non-empty string"): + validate_hardware_yaml(data) + + +def test_schema_rejects_empty_sensor_type(): + data = _load_mapping() + data["sensors"][0]["type"] = "" + with pytest.raises(ValueError, match=r"sensor .*: type must be a non-empty string"): + validate_hardware_yaml(data) + + +def test_schema_rejects_sensor_min_value_greater_than_max_value(): + data = _load_mapping() + data["sensors"][0]["min_value"] = 2000 + data["sensors"][0]["max_value"] = 0 + with pytest.raises(ValueError, match=r"sensor .*: min_value .* must be <= max_value"): + validate_hardware_yaml(data) + + +def test_schema_reports_missing_virtual_pin_when_gap_exists(): + data = _load_mapping() + data["actuators"][2]["virtual_pin"] = 3 + with pytest.raises(ValueError, match=r"board .*: missing virtual_pin indices \[[0-9, ]+\]"): + validate_hardware_yaml(data) + + +def test_schema_suppresses_sensor_contiguity_when_sensor_has_item_error(): + data = _load_mapping() + data["sensors"][0]["associated_actuator"] = "does_not_exist" + with pytest.raises(ValueError) as exc: + validate_hardware_yaml(data) + msg = str(exc.value) + assert "associated_actuator does_not_exist not found" in msg + assert "sensor virtual_pin must be contiguous" not in msg + assert "missing sensor virtual_pin indices" not in msg + + +def test_golden_firmware_left_arm(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"firmware"}, None)[ + "config_rp2040_left_arm.c" + ] + expected = (_FIXTURES / "golden_config_rp2040_left_arm.c").read_text(encoding="utf-8") + assert got == expected + + +def test_golden_firmware_right_arm(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"firmware"}, None)[ + "config_rp2040_right_arm.c" + ] + expected = (_FIXTURES / "golden_config_rp2040_right_arm.c").read_text(encoding="utf-8") + assert got == expected + + +def test_golden_firmware_torso(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"firmware"}, None)[ + "config_rp2040_torso_head.c" + ] + expected = (_FIXTURES / "golden_config_rp2040_torso_head.c").read_text(encoding="utf-8") + assert got == expected + + +def test_golden_ros2_control(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"ros2_control"}, None)[ + GENERATED_FILES_DEFAULTS["ros2_control_xacro"] + ] + expected = (_FIXTURES / "golden_inmoov_ros2_control.xacro").read_text(encoding="utf-8") + assert got == expected + + +def test_ros2_control_emits_urdf_limits_on_command_interface(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"ros2_control"}, None)[ + GENERATED_FILES_DEFAULTS["ros2_control_xacro"] + ] + assert 'name="left_shoulder_y_link_joint"' in got + assert '0.0' in got + assert '1.0' in got + # Every actuated revolute in the fixture has limits 0..1 rad. + assert got.count('') == 5 + assert got.count('') == 5 + + +def test_golden_controllers_extra_joints(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests(data, _fixture_urdf_xml(), {"controllers"}, None)[ + GENERATED_FILES_DEFAULTS["controllers_yaml"] + ] + expected = (_FIXTURES / "golden_controllers.yaml").read_text(encoding="utf-8") + assert got == expected + + +def test_boards_filter_emits_subset(): + data = _load_mapping() + got = generate_from_xacro_string_for_tests( + data, + _fixture_urdf_xml(), + {"firmware"}, + {"rp2040_left_arm"}, + ) + assert set(got.keys()) == {"config_rp2040_left_arm.c"} + + +def test_generated_files_defaults_when_section_absent(): + assert resolve_generated_files(_load_mapping()) == GENERATED_FILES_DEFAULTS + + +def test_generated_files_override_basenames(): + data = _load_mapping() + data["generated_files"] = { + "ros2_control_xacro": "thais_ros2_control.xacro", + "controllers_yaml": "thais_controllers.yaml", + } + assert resolve_generated_files(data) == { + "ros2_control_xacro": "thais_ros2_control.xacro", + "controllers_yaml": "thais_controllers.yaml", + } + + +def test_generated_files_rejects_path_separator(): + data = _load_mapping() + data["generated_files"] = {"controllers_yaml": "config/controllers.yaml"} + with pytest.raises(ValueError, match="bare filename"): + validate_hardware_yaml(data) + + +def test_generated_files_basenames_drive_output_keys(): + data = _load_mapping() + data["generated_files"] = { + "ros2_control_xacro": "thais_ros2_control.xacro", + "controllers_yaml": "thais_controllers.yaml", + } + got = generate_from_xacro_string_for_tests( + data, _fixture_urdf_xml(), {"ros2_control", "controllers"}, None + ) + assert "thais_ros2_control.xacro" in got + assert "thais_controllers.yaml" in got + assert GENERATED_FILES_DEFAULTS["ros2_control_xacro"] not in got diff --git a/lucy_config_generator/test/test_pep257.py b/lucy_config_generator/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/lucy_config_generator/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/lucy_config_generator/test/test_position_limits_unified.py b/lucy_config_generator/test/test_position_limits_unified.py new file mode 100644 index 0000000..7a9c8e5 --- /dev/null +++ b/lucy_config_generator/test/test_position_limits_unified.py @@ -0,0 +1,193 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +""" +URDF position limits in generated ros2_control (real + mock enforcement). + +The generated ``inmoov_ros2_control.xacro`` is consumed by different +```` implementations at xacro-expansion time: + +* ``gz_ros2_control/GazeboSimSystem`` (Gazebo) — receives the same ``min``/``max`` + params but stock upstream does **not** clamp in ``write()`` (we do not patch + ``gz_ros2_control`` in this workspace). +* ``lucy_ros2_control/LucySystemHardware`` with ``publish_actuators=false`` + (RViz / mock) — clamps ``hw_commands_`` to ``min``/``max``. +* ``lucy_ros2_control/LucySystemHardware`` with a micro-ROS publisher (real). + +These tests assert that ``min``/``max`` are emitted for every actuated joint, +match the URDF ````, and sit **outside** any ```` so real and +mock hardware always see the same command_interface envelope. +""" + +from __future__ import annotations + +import re +import xml.etree.ElementTree as ET +from pathlib import Path + +import yaml + +from lucy_config_generator.generate import generate_from_xacro_string_for_tests + +_FIXTURES = Path(__file__).resolve().parent / "fixtures" + + +def _load_mapping() -> dict: + with (_FIXTURES / "test_mapping.yaml").open(encoding="utf-8") as f: + return yaml.safe_load(f) + + +def _fixture_urdf_xml() -> str: + return (_FIXTURES / "test_robot.urdf.xacro").read_text(encoding="utf-8") + + +def _render_ros2_control(simulation_only: bool) -> str: + return generate_from_xacro_string_for_tests( + _load_mapping(), + _fixture_urdf_xml(), + targets={"ros2_control"}, + simulation_only=simulation_only, + )["inmoov_ros2_control.xacro"] + + +def _parse(xacro_xml: str) -> ET.Element: + """Parse the Jinja-rendered xacro as XML, declaring the xacro namespace.""" + # ``ros2_control.xacro.j2`` uses unprefixed ``xacro:`` element names that + # ``xml.etree`` would otherwise reject; declare the namespace so it parses. + decl = ' ET.Element: + cmd = joint_el.find("command_interface") + assert cmd is not None, f"joint {joint_el.attrib.get('name')!r} missing command_interface" + assert cmd.attrib.get("name") == "position" + return cmd + + +def _expected_limits_from_urdf(urdf_xml: str) -> dict[str, tuple[float, float]]: + root = ET.fromstring(urdf_xml) + limits: dict[str, tuple[float, float]] = {} + for joint in root.findall("joint"): + name = joint.attrib.get("name") + if joint.attrib.get("type") not in ("revolute", "prismatic"): + continue + limit = joint.find("limit") + if name is None or limit is None: + continue + lo = limit.attrib.get("lower") + hi = limit.attrib.get("upper") + if lo is None or hi is None: + continue + limits[name] = (float(lo), float(hi)) + return limits + + +def test_every_actuated_joint_emits_position_limits(): + """Property: every actuator row in the YAML gets ``min``/``max`` on its position command.""" + data = _load_mapping() + xacro = _render_ros2_control(simulation_only=False) + root = _parse(xacro) + + actuator_joints = {a["urdf_joint"] for a in data["actuators"]} + + seen: set[str] = set() + for joint_el in root.iter("joint"): + name = joint_el.attrib.get("name") + if name not in actuator_joints: + continue + seen.add(name) + cmd = _find_joint_command_interface(joint_el) + params = {p.attrib["name"]: p.text for p in cmd.findall("param")} + assert "min" in params, f"joint {name!r} command_interface missing " + assert "max" in params, f"joint {name!r} command_interface missing " + + assert seen == actuator_joints, ( + f"actuator joints not present in generated xacro: {actuator_joints - seen}" + ) + + +def test_position_limits_match_urdf(): + """The emitted ``min``/``max`` numerically match the URDF ```` (rad).""" + urdf_xml = _fixture_urdf_xml() + expected = _expected_limits_from_urdf(urdf_xml) + data = _load_mapping() + actuator_joints = {a["urdf_joint"] for a in data["actuators"]} + + xacro = _render_ros2_control(simulation_only=False) + root = _parse(xacro) + + for joint_el in root.iter("joint"): + name = joint_el.attrib.get("name") + if name not in actuator_joints or name not in expected: + continue + lo_expected, hi_expected = expected[name] + cmd = _find_joint_command_interface(joint_el) + params = {p.attrib["name"]: p.text for p in cmd.findall("param")} + assert float(params["min"]) == lo_expected, name + assert float(params["max"]) == hi_expected, name + + +def test_position_limits_apply_to_every_plugin_path(): + """ + Property: ``min``/``max`` are not gated on ``use_gazebo_sim`` / ``use_mock_hardware``. + + All three runtime plugins (GazeboSimSystem, LucySystemHardware-mock, + LucySystemHardware-real) read the same ```` block, so + the limit params must sit outside every ``xacro:if`` / ``xacro:unless``. + """ + xacro = _render_ros2_control(simulation_only=False) + + # Strip every ... and ... + # subtree, then re-check that limits remain. If a limit param were nested + # inside one of those gates it would disappear from this stripped view. + stripped = re.sub( + r"]*>.*?", + "", + xacro, + flags=re.DOTALL, + ) + assert stripped.count("') == xacro.count('') + assert stripped.count('') == xacro.count('') + + +def test_position_limits_identical_between_simulation_and_hardware_modes(): + """Generator must emit the same ``min``/``max`` set regardless of ``simulation_only``.""" + xacro_hw = _render_ros2_control(simulation_only=False) + xacro_sim = _render_ros2_control(simulation_only=True) + + def collect(xacro_xml: str) -> dict[str, tuple[str, str]]: + """Map joint name to (min, max) param text from generated xacro.""" + root = _parse(xacro_xml) + out: dict[str, tuple[str, str]] = {} + for joint_el in root.iter("joint"): + name = joint_el.attrib.get("name") + cmd = joint_el.find("command_interface") + if name is None or cmd is None: + continue + params = {p.attrib["name"]: p.text for p in cmd.findall("param")} + if "min" in params and "max" in params: + out[name] = (params["min"], params["max"]) + return out + + assert collect(xacro_hw) == collect(xacro_sim), ( + "URDF limits must match across simulation_only and hardware modes" + ) + + +def test_lucy_plugin_used_for_both_real_and_mock_hardware(): + """Real and mock both use ``LucySystemHardware`` for shared URDF clamping.""" + xacro = _render_ros2_control(simulation_only=False) + hw_plugin = "lucy_ros2_control/LucySystemHardware" + assert hw_plugin in xacro + assert "mock_components/GenericSystem" not in xacro + + # ``publish_actuators`` is the toggle used to suppress the micro-ROS + # actuator publisher in mock mode while keeping the same plugin (and the + # same URDF clamping) on both paths. + assert 'false' in xacro + assert '' in xacro diff --git a/lucy_config_generator/test/test_schema_optional_joint_lists.py b/lucy_config_generator/test/test_schema_optional_joint_lists.py new file mode 100644 index 0000000..dc9d4a9 --- /dev/null +++ b/lucy_config_generator/test/test_schema_optional_joint_lists.py @@ -0,0 +1,38 @@ +"""Optional root lists passive_urdf_joints / ignore_urdf_joints (and synonyms).""" + +from __future__ import annotations + +from pathlib import Path + +import pytest +import yaml + +from lucy_config_generator.schema import validate_hardware_yaml + + +_FIXTURE = Path(__file__).resolve().parent / "fixtures" / "test_mapping.yaml" + + +def _fixture_data() -> dict: + return yaml.safe_load(_FIXTURE.read_text(encoding="utf-8")) + + +def test_passive_and_ignore_urdf_joints_accepted(): + data = _fixture_data() + data["passive_urdf_joints"] = ["left_a_joint"] + data["ignore_urdf_joints"] = ["right_a_joint"] + validate_hardware_yaml(data) + + +def test_passive_ignore_synonym_keys_accepted(): + data = _fixture_data() + data["urdf_passive"] = ["left_a_joint"] + data["urdf_ignore"] = ["right_a_joint"] + validate_hardware_yaml(data) + + +def test_passive_urdf_joints_rejects_empty_string(): + data = _fixture_data() + data["passive_urdf_joints"] = ["ok", " "] + with pytest.raises(ValueError, match="passive_urdf_joints"): + validate_hardware_yaml(data) diff --git a/lucy_config_generator/test/test_simulation_only.py b/lucy_config_generator/test/test_simulation_only.py new file mode 100644 index 0000000..a38bfcf --- /dev/null +++ b/lucy_config_generator/test/test_simulation_only.py @@ -0,0 +1,61 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Simulation-only generation keeps the same 3 controllers and 1 ros2_control block per board.""" + +from __future__ import annotations + +from pathlib import Path + +import yaml + +from lucy_config_generator.generate import generate_from_xacro_string_for_tests + +_FIXTURES = Path(__file__).resolve().parent / "fixtures" + + +def _load_mapping() -> dict: + with (_FIXTURES / "test_mapping.yaml").open(encoding="utf-8") as f: + return yaml.safe_load(f) + + +def _fixture_urdf_xml() -> str: + return (_FIXTURES / "test_robot.urdf.xacro").read_text(encoding="utf-8") + + +def test_simulation_only_keeps_three_controllers_and_per_board_blocks(): + data = _load_mapping() + out = generate_from_xacro_string_for_tests( + data, + _fixture_urdf_xml(), + targets={"ros2_control", "controllers"}, + simulation_only=True, + ) + assert not any(k.startswith("config_") for k in out), \ + "simulation_only must not produce firmware" + + xacro = out["inmoov_ros2_control.xacro"] + expected_boards = list(data["boards"].keys()) + assert xacro.count(' -f --ser ` — the pipeline passes the UF2 under `firmware.build_dir`. **`picotool load` reboots the Pico into application mode**; a separate `picotool reboot` is **not** run (it races USB re-enumeration and often fails with exit **249**). +- Optional **1 s** pause after load (env `LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC`, set `0` to disable) before polling USB. +- Wait (default **5 s**, env `LUCY_PIPELINE_FLASH_WAIT_SEC`) until `/dev/serial/by-id/*` contains the serial (case-insensitive substring match). +- Wait for the next `std_msgs/msg/Int32` on the uptime topic (default **`/uptime_publisher`**, up to **30 s**, env `LUCY_PIPELINE_FLASH_UPTIME_WAIT_SEC`) so micro-ROS is publishing after boot. + +Shell aliases (e.g. `pico-flash-right-arm`) must use a **full path** to the `.uf2` or **`cd`** to the firmware `build/` directory first; otherwise picotool fails with **Could not open 'pico_micro_ros_right_arm.uf2'**. + +Override the uptime topic with env `LUCY_PIPELINE_UPTIME_TOPIC` or optional per-board YAML key `topic_uptime` (relative names get a leading `/`). + +Per-board isolation: one board failing load or wait steps does not stop other boards. Boards that failed **build** are skipped for flash; if **every** selected board failed build, the action aborts before flash. + +### Passwordless sudo for picotool + +`picotool` runs under **sudo**. On Ubuntu 22.04, for unattended flashing install a **sudoers drop-in** that whitelists the **`picotool` binary only** (use the path from `command -v picotool` / `which picotool`): + +```bash +PICOTOOL="$(command -v picotool)" +test -n "$PICOTOOL" && test -x "$PICOTOOL" || { echo "install picotool first"; exit 1; } + +echo "$USER ALL=(ALL) NOPASSWD: $PICOTOOL" | sudo tee /etc/sudoers.d/99-lucy-picotool +sudo chmod 0440 /etc/sudoers.d/99-lucy-picotool +sudo visudo -cf /etc/sudoers.d/99-lucy-picotool +``` + +If `visudo -cf` prints **syntax OK**, the rule is valid. Replace **`$USER`** with the account that runs the ROS node if you run this from a root shell where `$USER` is `root`. If `picotool` moves to another path, update the rule or re-run the block. + +Env: `LUCY_PIPELINE_FLASH_TIMEOUT_SEC` (default **120**) caps each `sudo picotool …` subprocess. + +## Services + +- `config/list` (`lucy_msgs/srv/ListConfigs`) +- `config/get` (`lucy_msgs/srv/GetConfig`) +- `config/save` (`lucy_msgs/srv/SaveConfig`) +- `config/activate` (`lucy_msgs/srv/ActivateConfig`) +- `config/delete` (`lucy_msgs/srv/DeleteConfig`) + +## Action + +- `configure_pipeline` (`lucy_msgs/action/ConfigurePipeline`) + +## Launch + +```bash +ros2 launch lucy_config_pipeline config_pipeline.launch.py robot_package:=thais_urdf +``` + +Optional: pass `config_dir:=/abs/path/to/config/hardware` to override source-path detection. diff --git a/lucy_config_pipeline/launch/config_pipeline.launch.py b/lucy_config_pipeline/launch/config_pipeline.launch.py new file mode 100644 index 0000000..a70802d --- /dev/null +++ b/lucy_config_pipeline/launch/config_pipeline.launch.py @@ -0,0 +1,23 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + robot_pkg_arg = DeclareLaunchArgument("robot_package", default_value="thais_urdf") + config_dir_arg = DeclareLaunchArgument("config_dir", default_value="") + + node = Node( + package="lucy_config_pipeline", + executable="config_pipeline_node", + output="screen", + parameters=[ + { + "robot_package": LaunchConfiguration("robot_package"), + "config_dir": LaunchConfiguration("config_dir"), + } + ], + ) + + return LaunchDescription([robot_pkg_arg, config_dir_arg, node]) diff --git a/lucy_config_pipeline/package.xml b/lucy_config_pipeline/package.xml new file mode 100644 index 0000000..8cea765 --- /dev/null +++ b/lucy_config_pipeline/package.xml @@ -0,0 +1,29 @@ + + + + lucy_config_pipeline + 0.0.0 + Config store services and ConfigurePipeline action server for Lucy hardware YAML pipeline. + Sentience Robotics Team + GPL-3.0 + + rclpy + std_srvs + std_msgs + rcl_interfaces + lucy_msgs + ament_index_python + python3-yaml + xacro + + lucy_config_generator + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/lucy_config_pipeline/resource/lucy_config_pipeline b/lucy_config_pipeline/resource/lucy_config_pipeline new file mode 100644 index 0000000..e69de29 diff --git a/lucy_config_pipeline/setup.cfg b/lucy_config_pipeline/setup.cfg new file mode 100644 index 0000000..9203369 --- /dev/null +++ b/lucy_config_pipeline/setup.cfg @@ -0,0 +1,10 @@ +[develop] +script_dir=$base/lib/lucy_config_pipeline +[install] +install_scripts=$base/lib/lucy_config_pipeline + +[flake8] +max-line-length = 100 + +[tool:pytest] +testpaths = test diff --git a/lucy_config_pipeline/setup.py b/lucy_config_pipeline/setup.py new file mode 100644 index 0000000..ef9e464 --- /dev/null +++ b/lucy_config_pipeline/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = "lucy_config_pipeline" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", ["launch/config_pipeline.launch.py"]), + ], + install_requires=["setuptools", "PyYAML", "Jinja2"], + zip_safe=True, + maintainer="Sentience Robotics Team", + maintainer_email="contact@sentience-robotics.fr", + description="Config store services + pipeline action server for Lucy hardware YAML.", + license="GPL-3.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "config_pipeline_node = src.main:main", + ], + }, +) diff --git a/lucy_config_pipeline/src/__init__.py b/lucy_config_pipeline/src/__init__.py new file mode 100644 index 0000000..f26f4d7 --- /dev/null +++ b/lucy_config_pipeline/src/__init__.py @@ -0,0 +1,7 @@ +"""Lucy config pipeline package.""" + +from .pipeline import PipelineActionServer, PipelinePaths +from .services.config_services_node import ConfigServicesNode +from .config_store import ConfigStore + +__all__ = ["PipelineActionServer", "PipelinePaths", "ConfigServicesNode", "ConfigStore"] diff --git a/lucy_config_pipeline/src/config_services_node.py b/lucy_config_pipeline/src/config_services_node.py new file mode 100644 index 0000000..c616698 --- /dev/null +++ b/lucy_config_pipeline/src/config_services_node.py @@ -0,0 +1,5 @@ +"""Backward-compatible import wrapper.""" + +from .services.config_services_node import ConfigServicesNode, main + +__all__ = ["ConfigServicesNode", "main"] diff --git a/lucy_config_pipeline/src/config_store.py b/lucy_config_pipeline/src/config_store.py new file mode 100644 index 0000000..443be30 --- /dev/null +++ b/lucy_config_pipeline/src/config_store.py @@ -0,0 +1,127 @@ +from __future__ import annotations + +from dataclasses import dataclass +from datetime import datetime +from pathlib import Path +import shutil +import yaml + + +@dataclass(frozen=True) +class ConfigStore: + config_dir: Path + + def __post_init__(self) -> None: + object.__setattr__(self, "config_dir", self.config_dir.resolve()) + + @property + def active_yaml(self) -> Path: + return self.config_dir / "active.yaml" + + @property + def active_meta_yaml(self) -> Path: + return self.config_dir / "active_meta.yaml" + + @property + def named_dir(self) -> Path: + return self.config_dir / "configs" + + @property + def backups_dir(self) -> Path: + return self.config_dir / "backups" + + def ensure_layout(self) -> None: + self.config_dir.mkdir(parents=True, exist_ok=True) + self.named_dir.mkdir(parents=True, exist_ok=True) + self.backups_dir.mkdir(parents=True, exist_ok=True) + + def list_configs(self) -> list[str]: + self.ensure_layout() + return sorted(p.stem for p in self.named_dir.glob("*.yaml")) + + def read_active_meta(self) -> dict: + if not self.active_meta_yaml.is_file(): + return {} + raw = self.active_meta_yaml.read_text(encoding="utf-8").strip() + if not raw: + return {} + data = yaml.safe_load(raw) + return data if isinstance(data, dict) else {} + + def _write_active_meta(self, meta: dict) -> None: + """Persist known keys only; preserves stable key order matching checked-in YAML style.""" + self.ensure_layout() + lines: list[str] = [] + for key in ("name", "activated_at", "flashed_name", "flashed_at"): + val = str(meta.get(key, "")).strip() + if val: + lines.append(f'{key}: "{val}"') + self.active_meta_yaml.write_text("\n".join(lines) + ("\n" if lines else ""), encoding="utf-8") + + def get_active_name(self) -> str: + data = self.read_active_meta() + name = str(data.get("name", "")).strip() + return name if name else "default" + + def get_flashed_name(self) -> str: + return str(self.read_active_meta().get("flashed_name", "")).strip() + + def get_flashed_at(self) -> str: + return str(self.read_active_meta().get("flashed_at", "")).strip() + + def record_flashed_preset(self, config_name: str) -> None: + meta = self.read_active_meta() + meta["flashed_name"] = config_name + meta["flashed_at"] = f"{datetime.utcnow().isoformat()}Z" + self._write_active_meta(meta) + + def _named_path(self, config_name: str) -> Path: + if not config_name or "/" in config_name or ".." in config_name: + raise ValueError("config_name must be a simple non-empty name") + return self.named_dir / f"{config_name}.yaml" + + def read_named_yaml(self, config_name: str) -> str: + path = self._named_path(config_name) + if not path.is_file(): + raise FileNotFoundError(path) + return path.read_text(encoding="utf-8") + + def read_active_yaml(self) -> str: + if not self.active_yaml.is_file(): + raise FileNotFoundError(self.active_yaml) + return self.active_yaml.read_text(encoding="utf-8") + + def write_named_yaml(self, config_name: str, config_yaml: str) -> Path: + self.ensure_layout() + path = self._named_path(config_name) + path.write_text(config_yaml, encoding="utf-8") + return path + + def activate(self, config_name: str) -> str: + self.ensure_layout() + src = self._named_path(config_name) + if not src.is_file(): + raise FileNotFoundError(src) + + backup_name = "" + if self.active_yaml.is_file(): + old = self.get_active_name() + ts = datetime.utcnow().strftime("%Y%m%d_%H%M%S") + backup_name = f"{ts}_{old or 'active'}" + shutil.copy2(self.active_yaml, self.backups_dir / f"{backup_name}.yaml") + + shutil.copy2(src, self.active_yaml) + meta = self.read_active_meta() + meta["name"] = config_name + meta["activated_at"] = f"{datetime.utcnow().isoformat()}Z" + self._write_active_meta(meta) + return backup_name + + def delete(self, config_name: str) -> None: + active = self.get_active_name() + if config_name in {"default", active}: + raise ValueError("cannot delete default or active config") + path = self._named_path(config_name) + if not path.is_file(): + raise FileNotFoundError(path) + path.unlink() diff --git a/lucy_config_pipeline/src/error_format.py b/lucy_config_pipeline/src/error_format.py new file mode 100644 index 0000000..3dabe09 --- /dev/null +++ b/lucy_config_pipeline/src/error_format.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +import json +import re + + +_BOARD_RE = re.compile(r"^board\s+([A-Za-z0-9_]+):\s+(.+)$") +_ACTUATOR_RE = re.compile(r"^actuator\s+([A-Za-z0-9_]+):\s+(.+)$") +_SENSOR_RE = re.compile(r"^sensor\s+([A-Za-z0-9_]+):\s+(.+)$") +_MISSING_ROOT_RE = re.compile(r"^missing root key:\s+([A-Za-z0-9_]+)$") +_LINE_COL_RE = re.compile(r"line\s+(\d+),\s+column\s+(\d+)") + + +def _pick_field_from_tail(tail: str) -> str: + if tail.startswith("missing virtual_pin"): + return "virtual_pin" + if tail.startswith("missing sensor virtual_pin"): + return "virtual_pin" + first = tail.split(" ", 1)[0] + if "." in first: + return first + if first.isidentifier(): + return first + return "unknown" + + +def format_error_line(line: str) -> str: + text = line.strip() + if not text: + return json.dumps( + {"field": "unknown", "field_path": ["unknown"], "message": "empty error line"} + ) + + m = _MISSING_ROOT_RE.match(text) + if m: + key = m.group(1) + return json.dumps( + {"field": key, "field_path": [key], "message": f"missing root key: {key}"} + ) + + m = _LINE_COL_RE.search(text) + if m: + line_no = int(m.group(1)) + col_no = int(m.group(2)) + return json.dumps( + { + "field": "yaml.syntax", + "field_path": ["yaml", "syntax"], + "line": line_no, + "column": col_no, + "message": text, + } + ) + + m = _BOARD_RE.match(text) + if m: + board_id, tail = m.groups() + field_tail = _pick_field_from_tail(tail) + field = f"boards.{board_id}.{field_tail}" + return json.dumps({"field": field, "field_path": field.split("."), "message": tail}) + + m = _ACTUATOR_RE.match(text) + if m: + act_id, tail = m.groups() + field_tail = _pick_field_from_tail(tail) + field = f"actuators.{act_id}.{field_tail}" + return json.dumps({"field": field, "field_path": field.split("."), "message": tail}) + + m = _SENSOR_RE.match(text) + if m: + sensor_id, tail = m.groups() + field_tail = _pick_field_from_tail(tail) + field = f"sensors.{sensor_id}.{field_tail}" + return json.dumps({"field": field, "field_path": field.split("."), "message": tail}) + + return json.dumps({"field": "unknown", "field_path": ["unknown"], "message": text}) + + +def format_error_lines(lines: list[str]) -> list[str]: + return [format_error_line(line) for line in lines if line and line.strip()] diff --git a/lucy_config_pipeline/src/main.py b/lucy_config_pipeline/src/main.py new file mode 100644 index 0000000..6b1b0a1 --- /dev/null +++ b/lucy_config_pipeline/src/main.py @@ -0,0 +1,108 @@ +from __future__ import annotations + +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +from lucy_config_generator.schema import GENERATED_FILES_DEFAULTS, resolve_generated_files +import rclpy +import yaml + +from .config_store import ConfigStore +from .pipeline.action_server import PipelineActionServer +from .pipeline.models import PipelinePaths +from .services.config_services_node import ConfigServicesNode + + +def _infer_robot_source_root(robot_package: str, share_dir: Path) -> Path: + cwd_candidate = Path.cwd() / "src" / robot_package + if cwd_candidate.is_dir(): + return cwd_candidate + + for p in share_dir.parents: + if p.name == "install": + src_candidate = p.parent / "src" / robot_package + if src_candidate.is_dir(): + return src_candidate + return share_dir + + +def _active_generated_files(cfg_dir: Path) -> dict[str, str]: + """Generated-artifact filenames from the active preset (defaults if unreadable).""" + active = cfg_dir / "active.yaml" + try: + data = yaml.safe_load(active.read_text(encoding="utf-8")) + if isinstance(data, dict): + return resolve_generated_files(data) + except (OSError, ValueError, yaml.YAMLError): + pass + return dict(GENERATED_FILES_DEFAULTS) + + +def _find_workspace_src(robot_root: Path) -> Path: + for p in robot_root.parents: + if p.name == "src": + return p + raise RuntimeError(f"cannot infer workspace src from {robot_root}") + + +def _resolve_paths(robot_package: str, config_dir: str) -> PipelinePaths: + share_dir = Path(get_package_share_directory(robot_package)) + robot_root = _infer_robot_source_root(robot_package, share_dir) + + cfg_dir = Path(config_dir).resolve() if config_dir else (robot_root / "config" / "hardware") + names = _active_generated_files(cfg_dir) + urdf_xacro = robot_root / "description" / "urdf" / "inmoov.urdf.xacro" + base_path = robot_root / "description" + controller_config = robot_root / "config" / names["controllers_yaml"] + + return PipelinePaths( + config_dir=cfg_dir, + urdf_xacro=urdf_xacro, + base_path=base_path, + controller_config=controller_config, + robot_root=robot_root, + workspace_src=_find_workspace_src(robot_root), + ) + + +def main() -> None: + rclpy.init() + + bootstrap = rclpy.create_node("lucy_config_pipeline_bootstrap") + bootstrap.declare_parameter("robot_package", "thais_urdf") + bootstrap.declare_parameter("config_dir", "") + robot_package = bootstrap.get_parameter("robot_package").get_parameter_value().string_value + config_dir = bootstrap.get_parameter("config_dir").get_parameter_value().string_value + paths = _resolve_paths(robot_package, config_dir) + bootstrap.destroy_node() + + store = ConfigStore(paths.config_dir) + store.ensure_layout() + + services_node = ConfigServicesNode( + robot_package=robot_package, + config_store=store, + urdf_xacro=paths.urdf_xacro, + base_path=paths.base_path, + controller_config=paths.controller_config, + ) + action_node = PipelineActionServer(paths=paths, config_store=store) + + # 4 threads: 1 for the action's _execute (which blocks for minutes during + # build/flash and seconds during reload), 1 for the reload service-client + # response callback that unblocks _execute, and 2 for the config services + # node so config/get/list/save remain responsive during a pipeline run. + executor = rclpy.executors.MultiThreadedExecutor(num_threads=4) + executor.add_node(services_node) + executor.add_node(action_node) + try: + executor.spin() + finally: + executor.shutdown() + services_node.destroy_node() + action_node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/lucy_config_pipeline/src/pipeline/__init__.py b/lucy_config_pipeline/src/pipeline/__init__.py new file mode 100644 index 0000000..4639fc9 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/__init__.py @@ -0,0 +1,6 @@ +"""Pipeline action server and build helpers.""" + +from .action_server import PipelineActionServer +from .models import PipelinePaths, FirmwarePaths + +__all__ = ["PipelineActionServer", "PipelinePaths", "FirmwarePaths"] diff --git a/lucy_config_pipeline/src/pipeline/action_server.py b/lucy_config_pipeline/src/pipeline/action_server.py new file mode 100644 index 0000000..0d34f96 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/action_server.py @@ -0,0 +1,361 @@ +from __future__ import annotations + +import json +import os +from pathlib import Path +import shutil +import tempfile +import threading + +from lucy_config_generator.generate import generate +from lucy_config_generator.schema import resolve_generated_files +from lucy_msgs.action import ConfigurePipeline +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from rclpy.node import Node +from std_srvs.srv import Trigger + +from ..config_store import ConfigStore +from ..error_format import format_error_lines +from ..validation import urdf_crosscheck, validate_schema +from .build import run_build_phase +from .flash import ( + flash_picotool_timeout_seconds, + flash_uptime_wait_seconds, + flash_usb_wait_seconds, + run_flash_phase, +) +from .models import PipelinePaths +from .selection import board_build_plan, resolve_mapping_input, select_boards_to_process + + +class PipelineActionServer(Node): + _BUILD_TIMEOUT_SECONDS = int(os.environ.get("LUCY_PIPELINE_BUILD_TIMEOUT_SEC", "300")) + _RELOAD_SERVICE = "/lucy_control/restart" + _RELOAD_TIMEOUT_SECONDS = 15.0 + + def __init__(self, *, paths: PipelinePaths, config_store: ConfigStore): + super().__init__("lucy_config_pipeline") + self._paths = paths + self._store = config_store + self._busy_lock = threading.Lock() + self._busy = False + # The action's _execute callback blocks while waiting for the reload + # service response. To avoid a single-callback-group deadlock (and the + # related corruption from re-entering the executor via + # spin_until_future_complete), give the action and the reload client + # independent callback groups so the response can be dispatched on a + # different thread while _execute is still running. + self._action_cb_group = MutuallyExclusiveCallbackGroup() + self._reload_cb_group = ReentrantCallbackGroup() + self._reload_client = self.create_client( + Trigger, self._RELOAD_SERVICE, callback_group=self._reload_cb_group + ) + self._action_server = ActionServer( + self, + ConfigurePipeline, + "configure_pipeline", + execute_callback=self._execute, + goal_callback=self._goal_callback, + cancel_callback=self._cancel_callback, + callback_group=self._action_cb_group, + ) + + def destroy_node(self) -> bool: + self._action_server.destroy() + return super().destroy_node() + + def _goal_callback(self, _goal_request: ConfigurePipeline.Goal) -> GoalResponse: + with self._busy_lock: + if self._busy: + return GoalResponse.REJECT + self._busy = True + return GoalResponse.ACCEPT + + def _cancel_callback(self, _goal_handle) -> CancelResponse: + return CancelResponse.ACCEPT + + def _feedback( + self, + goal_handle, + *, + phase: str, + progress: float, + detail: str, + board: str = "", + ) -> None: + fb = ConfigurePipeline.Feedback() + fb.phase = phase + q = round(max(0.0, min(1.0, float(progress))), 2) + fb.progress = q + fb.detail = json.dumps( + { + "phase": phase, + "board": board, + "progress": q, + "message": detail, + } + ) + fb.board = board + goal_handle.publish_feedback(fb) + + def _call_reload_service(self, goal_handle) -> tuple[bool, str]: + if not self._reload_client.wait_for_service(timeout_sec=self._RELOAD_TIMEOUT_SECONDS): + return False, f"service {self._RELOAD_SERVICE} not available" + + self._feedback( + goal_handle, + phase="reload", + progress=0.0, + detail="restarting control stack", + ) + req = Trigger.Request() + future = self._reload_client.call_async(req) + + # Wait for the response *without* re-entering the executor. The reload + # client lives in its own ReentrantCallbackGroup, so the + # MultiThreadedExecutor can dispatch the response on a different thread + # while this _execute call stays parked here. + done_event = threading.Event() + future.add_done_callback(lambda _f: done_event.set()) + completed = done_event.wait(timeout=self._RELOAD_TIMEOUT_SECONDS) + if goal_handle.is_cancel_requested: + return False, "reload cancelled" + if not completed or not future.done(): + return False, f"reload timed out after {self._RELOAD_TIMEOUT_SECONDS}s" + resp = future.result() + if resp is None: + return False, "reload service call failed" + if not resp.success: + return False, resp.message or "reload failed" + self._feedback( + goal_handle, + phase="reload", + progress=1.0, + detail=resp.message or "reload complete", + ) + return True, resp.message or "reload complete" + + def _generate_to_dir( + self, + *, + out_dir: Path, + config_yaml: str, + boards_filter: set[str] | None, + targets: set[str], + simulation_only: bool, + ) -> None: + (out_dir / "input.yaml").write_text(config_yaml, encoding="utf-8") + generate( + input_yaml=out_dir / "input.yaml", + urdf_xacro=self._paths.urdf_xacro, + base_path=self._paths.base_path, + controller_config=self._paths.controller_config, + output_dir=out_dir, + targets=targets, + boards_filter=boards_filter, + simulation_only=simulation_only, + ) + + def _execute(self, goal_handle): + result = ConfigurePipeline.Result() + result.success = False + result.config_name = self._store.get_active_name() + result.boards_flashed = [] + result.errors = [] + + try: + goal = goal_handle.request + result.config_name, config_yaml = resolve_mapping_input(self._store, goal.mapping_file) + + self._feedback(goal_handle, phase="validate", progress=0.1, detail="schema validation") + data = validate_schema(config_yaml) + + for path in ( + self._paths.urdf_xacro, + self._paths.base_path, + self._paths.controller_config, + ): + if not path.exists(): + raise FileNotFoundError(path) + + self._feedback(goal_handle, phase="validate", progress=0.6, detail="urdf cross-check") + report = urdf_crosscheck( + data, + self._paths.urdf_xacro, + self._paths.base_path, + self._paths.controller_config, + ) + if report.errors: + result.errors.extend(format_error_lines(report.errors)) + result.message = "validation failed" + goal_handle.abort() + return result + + boards = select_boards_to_process(data, list(goal.boards_to_flash)) + boards_set = set(boards) if boards else None + + with tempfile.TemporaryDirectory(prefix="lucy_config_pipeline_") as tmp: + out_dir = Path(tmp) + + # ros2_control + controllers are always regenerated, regardless of + # simulation_only / build_only / dry_run + self._feedback( + goal_handle, + phase="generate", + progress=0.3, + detail=( + "rendering simulation ros2_control" + if goal.simulation_only + else "rendering ros2_control" + ), + ) + self._generate_to_dir( + out_dir=out_dir, + config_yaml=config_yaml, + boards_filter=None if goal.simulation_only else boards_set, + targets={"ros2_control", "controllers"}, + simulation_only=goal.simulation_only, + ) + if not goal.dry_run: + self._feedback( + goal_handle, + phase="generate", + progress=0.5, + detail="installing ros2_control outputs", + ) + self._install_ros2_control_outputs(out_dir, data) + + if not goal.simulation_only: + self._feedback( + goal_handle, + phase="generate", + progress=0.7, + detail="rendering firmware", + ) + self._generate_to_dir( + out_dir=out_dir, + config_yaml=config_yaml, + boards_filter=boards_set, + targets={"firmware"}, + simulation_only=False, + ) + if not goal.dry_run: + self._install_firmware_outputs(out_dir, data) + + build_failed_boards: list[str] = [] + if not goal.dry_run and not goal.simulation_only: + build_failed_boards = run_build_phase( + data=data, + selected_boards=boards, + workspace_src=self._paths.workspace_src, + timeout_seconds=self._BUILD_TIMEOUT_SECONDS, + feedback=lambda **kwargs: self._feedback(goal_handle, **kwargs), + log_error=lambda msg: self.get_logger().error(msg), + ) + + plan_boards = [b for b, _ in board_build_plan(data, boards)] + plan_set = set(plan_boards) + built_ok = {b for b in plan_boards if b not in set(build_failed_boards)} + + if not goal.dry_run and goal.build_only and build_failed_boards: + result.errors.extend( + format_error_lines( + [f"build failed for boards: {', '.join(sorted(build_failed_boards))}"] + ) + ) + result.message = "build failed" + goal_handle.abort() + return result + + if not goal.dry_run and plan_set and not built_ok and not goal.simulation_only: + result.errors.extend( + format_error_lines( + [f"build failed for boards: {', '.join(sorted(build_failed_boards))}"] + ) + ) + result.message = "build failed" + goal_handle.abort() + return result + + if build_failed_boards: + result.errors.extend( + format_error_lines( + [f"build failed for boards: {', '.join(sorted(build_failed_boards))}"] + ) + ) + + flash_failed: list[str] = [] + if not goal.dry_run and not goal.build_only and not goal.simulation_only: + flash_failed, flashed_ok = run_flash_phase( + data=data, + selected_boards=boards, + boards_built_ok=built_ok, + workspace_src=self._paths.workspace_src, + picotool_timeout_seconds=flash_picotool_timeout_seconds(), + usb_wait_seconds=flash_usb_wait_seconds(), + uptime_wait_seconds=flash_uptime_wait_seconds(), + node=self, + feedback=lambda **kwargs: self._feedback(goal_handle, **kwargs), + log_error=lambda msg: self.get_logger().error(msg), + ) + result.boards_flashed = flashed_ok + if flash_failed: + result.errors.extend( + format_error_lines( + [f"flash failed for boards: {', '.join(sorted(flash_failed))}"] + ) + ) + result.message = "flash failed" + goal_handle.abort() + return result + if flashed_ok: + self._store.record_flashed_preset(result.config_name) + + if not goal.dry_run: + ok, reload_msg = self._call_reload_service(goal_handle) + if not ok: + result.errors.extend(format_error_lines([reload_msg])) + result.message = reload_msg + goal_handle.abort() + return result + + result.success = True + if build_failed_boards: + result.message = "pipeline completed with partial build failure" + else: + result.message = "pipeline completed" + goal_handle.succeed() + return result + except Exception as e: # pragma: no cover - top-level action guard + lines = [line for line in str(e).splitlines() if line.strip()] + raw = lines if lines else [str(e)] + result.errors.extend(format_error_lines(raw)) + result.message = f"pipeline failed: {e}" + goal_handle.abort() + return result + finally: + with self._busy_lock: + self._busy = False + + def _install_ros2_control_outputs(self, out_dir: Path, data: dict) -> None: + names = resolve_generated_files(data) + xacro_name = names["ros2_control_xacro"] + ctrl_name = names["controllers_yaml"] + xacro_dst = ( + self._paths.robot_root / "description" / "ros2_control" / xacro_name + ) + ctrl_dst = self._paths.robot_root / "config" / ctrl_name + xacro_dst.parent.mkdir(parents=True, exist_ok=True) + ctrl_dst.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(out_dir / xacro_name, xacro_dst) + shutil.copy2(out_dir / ctrl_name, ctrl_dst) + + def _install_firmware_outputs(self, out_dir: Path, data: dict) -> None: + firmware_src_dir = str(data.get("firmware", {}).get("source_dir", "")).strip() + if not firmware_src_dir: + return + fw_cfg_dir = (self._paths.workspace_src / firmware_src_dir / "config").resolve() + fw_cfg_dir.mkdir(parents=True, exist_ok=True) + for cfile in out_dir.glob("config_*.c"): + shutil.copy2(cfile, fw_cfg_dir / cfile.name) diff --git a/lucy_config_pipeline/src/pipeline/build.py b/lucy_config_pipeline/src/pipeline/build.py new file mode 100644 index 0000000..3bb52c0 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/build.py @@ -0,0 +1,123 @@ +from __future__ import annotations + +from collections.abc import Callable +from pathlib import Path +import subprocess + +from .selection import board_build_plan, resolve_firmware_paths + + +def run_build_phase( + *, + data: dict, + selected_boards: set[str] | None, + workspace_src: Path, + timeout_seconds: int, + feedback: Callable[..., None], + log_error: Callable[[str], None], +) -> list[str]: + paths = resolve_firmware_paths(data, workspace_src) + if not paths.source_dir.exists(): + raise FileNotFoundError(paths.source_dir) + + paths.build_dir.mkdir(parents=True, exist_ok=True) + plan = board_build_plan(data, selected_boards) + total_steps = len(plan) + 1 + step_idx = 0 + + cmake_cache = paths.build_dir / "CMakeCache.txt" + if not cmake_cache.exists(): + feedback( + phase="build", + progress=step_idx / total_steps, + detail=f"running cmake in {paths.build_dir}", + board="", + ) + _run_command( + phase="build", + board="", + cmd=["cmake", ".."], + cwd=paths.build_dir, + timeout_seconds=timeout_seconds, + feedback=feedback, + stream_progress=step_idx / total_steps, + ) + step_idx += 1 + + failed_boards: list[str] = [] + for board, target in plan: + feedback( + phase="build", + progress=step_idx / total_steps, + detail=f"building target {target}", + board=board, + ) + try: + _run_command( + phase="build", + board=board, + cmd=["make", target], + cwd=paths.build_dir, + timeout_seconds=timeout_seconds, + feedback=feedback, + stream_progress=step_idx / total_steps, + ) + uf2 = paths.build_dir / f"{target}.uf2" + if not uf2.exists(): + raise RuntimeError(f"missing build artifact: {uf2}") + except Exception as exc: + failed_boards.append(board) + log_error(f"Build failed for {board}: {exc}") + feedback( + phase="build", + progress=step_idx / total_steps, + detail=f"build failed for {board}: {exc}", + board=board, + ) + finally: + step_idx += 1 + + feedback( + phase="build", + progress=1.0, + detail="build phase completed", + board="", + ) + return failed_boards + + +def _run_command( + *, + phase: str, + board: str, + cmd: list[str], + cwd: Path, + timeout_seconds: int, + feedback: Callable[..., None], + stream_progress: float = 0.0, +) -> None: + process = subprocess.Popen( + cmd, + cwd=str(cwd), + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, + ) + assert process.stdout is not None + emitted = 0 + for line in process.stdout: + text = line.strip() + if not text: + continue + if emitted < 200: + feedback(phase=phase, progress=stream_progress, detail=text, board=board) + emitted += 1 + try: + return_code = process.wait(timeout=timeout_seconds) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=5) + raise TimeoutError(f"command timed out after {timeout_seconds}s: {' '.join(cmd)}") + if return_code != 0: + raise RuntimeError(f"command failed ({return_code}): {' '.join(cmd)}") diff --git a/lucy_config_pipeline/src/pipeline/flash.py b/lucy_config_pipeline/src/pipeline/flash.py new file mode 100644 index 0000000..44408a2 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/flash.py @@ -0,0 +1,246 @@ +from __future__ import annotations + +from collections.abc import Callable +import os +from pathlib import Path +import subprocess +import time +from typing import TYPE_CHECKING + +from .selection import board_build_plan, resolve_firmware_paths + +if TYPE_CHECKING: + from rclpy.node import Node + + +def run_flash_phase( + *, + data: dict, + selected_boards: set[str] | None, + boards_built_ok: set[str], + workspace_src: Path, + picotool_timeout_seconds: int, + usb_wait_seconds: int, + uptime_wait_seconds: int, + node: Node | None, + feedback: Callable[..., None], + log_error: Callable[[str], None], +) -> tuple[list[str], list[str]]: + """ + Flash built UF2 images with picotool. + + Only boards present in ``boards_built_ok`` are considered. Boards without a + non-empty ``serial_id`` are skipped (not counted as failure). + + After USB serial re-enumeration, optionally waits for ``std_msgs/Int32`` on + the board uptime topic (see ``_uptime_topic``) when ``node`` is set. + + Returns ``(failed_board_ids, flashed_board_ids)`` in stable board order. + """ + paths = resolve_firmware_paths(data, workspace_src) + if not paths.build_dir.is_dir(): + raise FileNotFoundError(paths.build_dir) + + plan = board_build_plan(data, selected_boards) + flashable = [(b, t) for b, t in plan if b in boards_built_ok] + total = max(len(flashable), 1) + + failed: list[str] = [] + flashed: list[str] = [] + + for step_i, (board, target) in enumerate(flashable): + row = step_i / total + boards_entry = data.get("boards", {}).get(board, {}) + raw_serial = boards_entry.get("serial_id") + serial = str(raw_serial).strip() if raw_serial is not None else "" + if not serial: + feedback( + phase="flash", + progress=min(1.0, row + 0.99 / total), + detail=f"skip flash for {board}: no serial_id", + board=board, + ) + continue + + uf2 = paths.build_dir / f"{target}.uf2" + if not uf2.is_file(): + msg = f"missing UF2 for flash: {uf2}" + log_error(f"Flash failed for {board}: {msg}") + failed.append(board) + feedback( + phase="flash", + progress=min(1.0, row + 0.5 / total), + detail=msg, + board=board, + ) + continue + + try: + feedback( + phase="flash", + progress=row + 0.1 / total, + detail=f"picotool load {uf2.name}", + board=board, + ) + load_progress = min(1.0, row + 0.25 / total) + _run_command( + phase="flash", + board=board, + cmd=["sudo", "picotool", "load", str(uf2), "-f", "--ser", serial], + cwd=paths.build_dir, + timeout_seconds=picotool_timeout_seconds, + feedback=feedback, + stream_progress=load_progress, + ) + # ``picotool load`` already reboots the board into application mode; a + # follow-up ``picotool reboot`` races USB re-enumeration and often + # fails (e.g. exit 249) while the device is offline. + post_load_delay = float(os.environ.get("LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC", "1")) + if post_load_delay > 0: + time.sleep(post_load_delay) + feedback( + phase="flash", + progress=min(1.0, row + 0.5 / total), + detail=f"waiting up to {usb_wait_seconds}s for USB serial", + board=board, + ) + if not _wait_for_usb_serial(serial, usb_wait_seconds): + raise TimeoutError( + f"USB serial did not become ready within {usb_wait_seconds}s " + f"(board {board})" + ) + if node is not None and uptime_wait_seconds > 0: + topic = _uptime_topic(boards_entry) + feedback( + phase="flash", + progress=min(1.0, row + 0.75 / total), + detail=f"waiting for uptime on {topic} (up to {uptime_wait_seconds}s)", + board=board, + ) + if not _wait_uptime_message(node, topic, float(uptime_wait_seconds)): + raise TimeoutError( + f"no uptime message on {topic!r} within {uptime_wait_seconds}s " + f"(board {board})" + ) + flashed.append(board) + feedback( + phase="flash", + progress=min(1.0, row + 0.95 / total), + detail=f"flash completed for {board}", + board=board, + ) + except Exception as exc: + failed.append(board) + log_error(f"Flash failed for {board}: {exc}") + feedback( + phase="flash", + progress=min(1.0, row + 0.9 / total), + detail=f"flash failed for {board}: {exc}", + board=board, + ) + + feedback( + phase="flash", + progress=1.0, + detail="flash phase completed", + board="", + ) + return failed, flashed + + +def _uptime_topic(boards_entry: dict) -> str: + """Resolve absolute ROS 2 topic for ``std_msgs/msg/Int32`` uptime ticks.""" + raw = boards_entry.get("topic_uptime") + if isinstance(raw, str): + t = raw.strip() + if t: + return t if t.startswith("/") else f"/{t}" + env = os.environ.get("LUCY_PIPELINE_UPTIME_TOPIC", "").strip() + if env: + return env if env.startswith("/") else f"/{env}" + return "/uptime_publisher" + + +def _wait_uptime_message(node: Node, topic: str, timeout_sec: float) -> bool: + """ + Wait for one ``std_msgs/msg/Int32`` on ``topic`` within ``timeout_sec``. + + Returns True if a message was received, False on timeout. + """ + from rclpy.wait_for_message import wait_for_message + from std_msgs.msg import Int32 + + ok, _msg = wait_for_message(Int32, node, topic, time_to_wait=timeout_sec) + return bool(ok) + + +def _wait_for_usb_serial(serial_id: str, timeout_seconds: int) -> bool: + """Return whether a /dev/serial/by-id entry containing ``serial_id`` resolves.""" + needle = serial_id.strip().lower() + if not needle: + return False + deadline = time.monotonic() + max(0.1, float(timeout_seconds)) + by_id = Path("/dev/serial/by-id") + while time.monotonic() < deadline: + if by_id.is_dir(): + for entry in by_id.iterdir(): + name = entry.name.lower() + if needle not in name: + continue + try: + resolved = entry.resolve() + if resolved.exists(): + return True + except OSError: + continue + time.sleep(0.5) + return False + + +def _run_command( + *, + phase: str, + board: str, + cmd: list[str], + cwd: Path, + timeout_seconds: int, + feedback: Callable[..., None], + stream_progress: float = 0.0, +) -> None: + process = subprocess.Popen( + cmd, + cwd=str(cwd), + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, + ) + assert process.stdout is not None + emitted = 0 + for line in process.stdout: + text = line.strip() + if not text: + continue + if emitted < 200: + feedback(phase=phase, progress=stream_progress, detail=text, board=board) + emitted += 1 + try: + return_code = process.wait(timeout=timeout_seconds) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=5) + raise TimeoutError(f"command timed out after {timeout_seconds}s: {' '.join(cmd)}") + if return_code != 0: + raise RuntimeError(f"command failed ({return_code}): {' '.join(cmd)}") + + +def flash_picotool_timeout_seconds() -> int: + return int(os.environ.get("LUCY_PIPELINE_FLASH_TIMEOUT_SEC", "120")) + + +def flash_usb_wait_seconds() -> int: + return int(os.environ.get("LUCY_PIPELINE_FLASH_WAIT_SEC", "5")) + + +def flash_uptime_wait_seconds() -> int: + return int(os.environ.get("LUCY_PIPELINE_FLASH_UPTIME_WAIT_SEC", "30")) diff --git a/lucy_config_pipeline/src/pipeline/models.py b/lucy_config_pipeline/src/pipeline/models.py new file mode 100644 index 0000000..e27b9f3 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/models.py @@ -0,0 +1,20 @@ +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class PipelinePaths: + config_dir: Path + urdf_xacro: Path + base_path: Path + controller_config: Path + robot_root: Path + workspace_src: Path + + +@dataclass(frozen=True) +class FirmwarePaths: + source_dir: Path + build_dir: Path diff --git a/lucy_config_pipeline/src/pipeline/selection.py b/lucy_config_pipeline/src/pipeline/selection.py new file mode 100644 index 0000000..0c35610 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline/selection.py @@ -0,0 +1,56 @@ +from __future__ import annotations + +from pathlib import Path + +from ..config_store import ConfigStore +from .models import FirmwarePaths + + +def resolve_mapping_input(store: ConfigStore, mapping_file: str) -> tuple[str, str]: + if mapping_file: + mapping_path = Path(mapping_file) + if mapping_file.endswith(".yaml") and mapping_path.is_file(): + return mapping_path.stem, mapping_path.read_text(encoding="utf-8") + return mapping_file, store.read_named_yaml(mapping_file) + return store.get_active_name(), store.read_active_yaml() + + +def select_boards_to_process(data: dict, requested: list[str]) -> set[str] | None: + if not requested: + return None + boards = {b for b in requested if b} + known = set(data.get("boards", {}).keys()) + unknown = sorted(boards - known) + if unknown: + raise ValueError(f"unknown boards requested: {', '.join(unknown)}") + return boards + + +def resolve_firmware_paths(data: dict, workspace_src: Path) -> FirmwarePaths: + firmware = data.get("firmware", {}) + source_dir_raw = str(firmware.get("source_dir", "")).strip() + if not source_dir_raw: + raise ValueError("missing firmware.source_dir in mapping") + + build_dir_raw = str(firmware.get("build_dir", "")).strip() or "build" + source_dir = Path(source_dir_raw) + if not source_dir.is_absolute(): + source_dir = (workspace_src / source_dir).resolve() + + build_dir = Path(build_dir_raw) + if not build_dir.is_absolute(): + build_dir = (source_dir / build_dir).resolve() + + return FirmwarePaths(source_dir=source_dir, build_dir=build_dir) + + +def board_build_plan(data: dict, selected_boards: set[str] | None) -> list[tuple[str, str]]: + boards_map = data.get("boards", {}) + board_ids = sorted(selected_boards) if selected_boards else sorted(boards_map.keys()) + plan: list[tuple[str, str]] = [] + for board in board_ids: + firmware_target = str(boards_map.get(board, {}).get("firmware_target", "")).strip() + if not firmware_target: + raise ValueError(f"missing firmware_target for board '{board}'") + plan.append((board, firmware_target)) + return plan diff --git a/lucy_config_pipeline/src/pipeline_action_server.py b/lucy_config_pipeline/src/pipeline_action_server.py new file mode 100644 index 0000000..b8fe2bd --- /dev/null +++ b/lucy_config_pipeline/src/pipeline_action_server.py @@ -0,0 +1,12 @@ +"""Backward-compatible import wrapper.""" + +from .pipeline.action_server import PipelineActionServer +from .pipeline.models import PipelinePaths +from .pipeline.selection import resolve_mapping_input, select_boards_to_process + +__all__ = [ + "PipelineActionServer", + "PipelinePaths", + "resolve_mapping_input", + "select_boards_to_process", +] diff --git a/lucy_config_pipeline/src/pipeline_build.py b/lucy_config_pipeline/src/pipeline_build.py new file mode 100644 index 0000000..078cde6 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline_build.py @@ -0,0 +1,5 @@ +"""Backward-compatible import wrapper.""" + +from .pipeline.build import run_build_phase + +__all__ = ["run_build_phase"] diff --git a/lucy_config_pipeline/src/pipeline_models.py b/lucy_config_pipeline/src/pipeline_models.py new file mode 100644 index 0000000..fa30a52 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline_models.py @@ -0,0 +1,5 @@ +"""Backward-compatible import wrapper.""" + +from .pipeline.models import FirmwarePaths, PipelinePaths + +__all__ = ["PipelinePaths", "FirmwarePaths"] diff --git a/lucy_config_pipeline/src/pipeline_selection.py b/lucy_config_pipeline/src/pipeline_selection.py new file mode 100644 index 0000000..f5dfc13 --- /dev/null +++ b/lucy_config_pipeline/src/pipeline_selection.py @@ -0,0 +1,15 @@ +"""Backward-compatible import wrapper.""" + +from .pipeline.selection import ( + board_build_plan, + resolve_firmware_paths, + resolve_mapping_input, + select_boards_to_process, +) + +__all__ = [ + "resolve_mapping_input", + "select_boards_to_process", + "resolve_firmware_paths", + "board_build_plan", +] diff --git a/lucy_config_pipeline/src/services/__init__.py b/lucy_config_pipeline/src/services/__init__.py new file mode 100644 index 0000000..6a347f6 --- /dev/null +++ b/lucy_config_pipeline/src/services/__init__.py @@ -0,0 +1 @@ +"""Service nodes for config management.""" diff --git a/lucy_config_pipeline/src/services/config_services_node.py b/lucy_config_pipeline/src/services/config_services_node.py new file mode 100644 index 0000000..fe2af79 --- /dev/null +++ b/lucy_config_pipeline/src/services/config_services_node.py @@ -0,0 +1,208 @@ +from __future__ import annotations + +from pathlib import Path + +from lucy_msgs.srv import ( + ActivateConfig, + DeleteConfig, + GetConfig, + GetInt, + GetMesh, + ListConfigs, + SaveConfig, +) +import rclpy +from rclpy.node import Node +from std_msgs.msg import Int32 + +from ..config_store import ConfigStore +from ..error_format import format_error_lines +from ..validation import urdf_crosscheck, validate_schema + + +class ConfigServicesNode(Node): + def __init__( + self, + *, + robot_package: str, + config_store: ConfigStore, + urdf_xacro: Path, + base_path: Path, + controller_config: Path, + ): + super().__init__("lucy_config_services") + self._robot_package = robot_package + self._store = config_store + self._urdf_xacro = urdf_xacro + self._base_path = base_path + self._controller_config = controller_config + + self.create_service(ListConfigs, "config/list", self._on_list_configs) + self.create_service(GetConfig, "config/get", self._on_get_config) + self.create_service(SaveConfig, "config/save", self._on_save_config) + self.create_service(ActivateConfig, "config/activate", self._on_activate_config) + self.create_service(DeleteConfig, "config/delete", self._on_delete_config) + self.create_service(GetMesh, "mesh/get", self._on_get_mesh) + + self._client_count: int = 0 + self.create_subscription(Int32, "/client_count", self._on_client_count, 10) + self.create_service(GetInt, "/get_client_count", self._on_get_client_count) + + def _on_client_count(self, msg: Int32) -> None: + self._client_count = msg.data + + def _on_get_client_count( + self, _req: GetInt.Request, res: GetInt.Response + ) -> GetInt.Response: + res.value = self._client_count + return res + + def _on_list_configs( + self, _req: ListConfigs.Request, res: ListConfigs.Response + ) -> ListConfigs.Response: + try: + res.config_names = self._store.list_configs() + res.active_config = self._store.get_active_name() + res.success = True + res.message = "ok" + except Exception as e: # pragma: no cover - defensive wrapper + res.success = False + res.message = str(e) + return res + + def _on_get_config( + self, req: GetConfig.Request, res: GetConfig.Response + ) -> GetConfig.Response: + try: + if req.config_name: + res.config_name = req.config_name + res.config_yaml = self._store.read_named_yaml(req.config_name) + else: + res.config_name = self._store.get_active_name() + res.config_yaml = self._store.read_active_yaml() + res.robot_package = self._robot_package + res.flashed_config_name = self._store.get_flashed_name() + res.flashed_at = self._store.get_flashed_at() + res.success = True + res.message = "ok" + except FileNotFoundError: + res.success = False + res.message = "Configuration not found" + res.robot_package = self._robot_package + res.config_name = "" + res.config_yaml = "" + res.flashed_config_name = "" + res.flashed_at = "" + except Exception as e: + res.success = False + res.message = str(e) + res.robot_package = self._robot_package + res.config_name = "" + res.config_yaml = "" + res.flashed_config_name = "" + res.flashed_at = "" + return res + + def _on_save_config( + self, req: SaveConfig.Request, res: SaveConfig.Response + ) -> SaveConfig.Response: + try: + data = validate_schema(req.config_yaml) + report = urdf_crosscheck( + data, + self._urdf_xacro, + self._base_path, + self._controller_config, + ) + res.urdf_warnings = report.warnings + if report.errors: + res.validation_errors = report.errors + res.success = False + res.message = "validation failed" + return res + + self._store.write_named_yaml(req.config_name, req.config_yaml) + if req.activate: + self._store.activate(req.config_name) + res.success = True + res.message = "saved" + except ValueError as e: + res.success = False + res.message = "validation failed" + lines = [line for line in str(e).splitlines() if line.strip()] + res.validation_errors = format_error_lines(lines) + except Exception as e: + res.success = False + res.message = str(e) + return res + + def _on_activate_config( + self, req: ActivateConfig.Request, res: ActivateConfig.Response + ) -> ActivateConfig.Response: + try: + res.backup_name = self._store.activate(req.config_name) + res.success = True + res.message = "activated" + except Exception as e: + res.success = False + res.message = str(e) + return res + + def _on_delete_config( + self, req: DeleteConfig.Request, res: DeleteConfig.Response + ) -> DeleteConfig.Response: + try: + self._store.delete(req.config_name) + res.success = True + res.message = "deleted" + except Exception as e: + res.success = False + res.message = str(e) + return res + + def _on_get_mesh(self, req: GetMesh.Request, res: GetMesh.Response) -> GetMesh.Response: + # Serves the mesh files referenced by the URDF (file:// / package:// / + # absolute) so the web viewer can fetch them over ROS instead of the + # filesystem. Reads are confined to the robot's description tree. + try: + candidate = self._mesh_path_from_ref(req.path) + base = self._base_path.resolve() + if base != candidate and base not in candidate.parents: + res.success = False + res.message = f"path outside robot description: {candidate}" + return res + if not candidate.is_file(): + res.success = False + res.message = f"mesh not found: {candidate}" + return res + res.data = candidate.read_text() + res.success = True + res.message = "ok" + except Exception as e: + res.success = False + res.message = str(e) + return res + + def _mesh_path_from_ref(self, ref: str) -> Path: + """Resolve a URDF mesh reference to an absolute filesystem path.""" + if ref.startswith("file://"): + return Path(ref[len("file://"):]).resolve() + if ref.startswith("package://"): + # package:/// — robot packages are rooted at base_path's parent. + _, _, rel = ref[len("package://"):].partition("/") + return (self._base_path.parent / rel).resolve() + return Path(ref).resolve() + + +def main() -> None: # pragma: no cover + rclpy.init() + node = ConfigServicesNode( + robot_package="thais_urdf", + config_store=ConfigStore(Path(".")), + urdf_xacro=Path("inmoov.urdf.xacro"), + base_path=Path("."), + controller_config=Path("controllers.yaml"), + ) + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/lucy_config_pipeline/src/validation.py b/lucy_config_pipeline/src/validation.py new file mode 100644 index 0000000..62e195b --- /dev/null +++ b/lucy_config_pipeline/src/validation.py @@ -0,0 +1,64 @@ +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import yaml + +from lucy_config_generator.generate import urdf_joint_names +from lucy_config_generator.schema import ( + URDF_IGNORE_LIST_KEYS, + URDF_PASSIVE_LIST_KEYS, + validate_hardware_yaml, +) + + +@dataclass(frozen=True) +class ValidationReport: + errors: list[str] + warnings: list[str] + + +def parse_yaml_text(config_yaml: str) -> dict[str, Any]: + data = yaml.safe_load(config_yaml) + if not isinstance(data, dict): + raise ValueError("YAML root must be a mapping") + return data + + +def validate_schema(config_yaml: str) -> dict[str, Any]: + data = parse_yaml_text(config_yaml) + validate_hardware_yaml(data) + return data + + +def urdf_crosscheck( + data: dict[str, Any], + urdf_xacro: Path, + base_path: Path, + controller_config: Path, +) -> ValidationReport: + errors: list[str] = [] + warnings: list[str] = [] + joints = urdf_joint_names(urdf_xacro, base_path, controller_config) + + actuated = set() + for a in data.get("actuators", []): + j = str(a.get("urdf_joint", "")).strip() + if not j: + continue + actuated.add(j) + if j not in joints: + errors.append(f"actuator {a.get('id')}: urdf_joint {j!r} not in URDF") + + passive_ignore = set() + for key in URDF_PASSIVE_LIST_KEYS + URDF_IGNORE_LIST_KEYS: + for x in data.get(key) or []: + if isinstance(x, str) and x.strip(): + passive_ignore.add(x.strip()) + + for j in sorted(joints - actuated - passive_ignore): + warnings.append(f"URDF joint {j!r} is not mapped to any actuator") + + return ValidationReport(errors=errors, warnings=warnings) diff --git a/lucy_config_pipeline/test/conftest.py b/lucy_config_pipeline/test/conftest.py new file mode 100644 index 0000000..79a3a00 --- /dev/null +++ b/lucy_config_pipeline/test/conftest.py @@ -0,0 +1,10 @@ +"""Prefer this package's `src/` tree over an older site-packages copy named `src`.""" + +from __future__ import annotations + +import sys +from pathlib import Path + +_PIPELINE_ROOT = Path(__file__).resolve().parents[1] +if _PIPELINE_ROOT.is_dir(): + sys.path.insert(0, str(_PIPELINE_ROOT)) diff --git a/lucy_config_pipeline/test/formatting/test_error_format.py b/lucy_config_pipeline/test/formatting/test_error_format.py new file mode 100644 index 0000000..1d59364 --- /dev/null +++ b/lucy_config_pipeline/test/formatting/test_error_format.py @@ -0,0 +1,52 @@ +import json + +from src.error_format import format_error_line + + +def test_board_error_formats_to_json_field_path(): + s = format_error_line( + "board rp2040_left_arm: controller.name must be a non-empty string" + ) + o = json.loads(s) + assert o["field"] == "boards.rp2040_left_arm.controller.name" + assert o["field_path"] == ["boards", "rp2040_left_arm", "controller", "name"] + assert "non-empty" in o["message"] + + +def test_missing_root_key_formats_to_json(): + s = format_error_line("missing root key: boards") + o = json.loads(s) + assert o["field"] == "boards" + assert o["field_path"] == ["boards"] + assert "missing root key" in o["message"] + + +def test_yaml_parser_line_column_are_exposed(): + s = format_error_line('in "", line 67, column 18:') + o = json.loads(s) + assert o["field"] == "yaml.syntax" + assert o["field_path"] == ["yaml", "syntax"] + assert o["line"] == 67 + assert o["column"] == 18 + + +def test_board_virtual_pin_gap_message_is_preserved(): + s = format_error_line( + "board rp2040_left_arm: virtual_pin must be contiguous from 0..N-1, got [0, 1, 3]" + ) + o = json.loads(s) + assert o["field"] == "boards.rp2040_left_arm.virtual_pin" + assert o["field_path"] == ["boards", "rp2040_left_arm", "virtual_pin"] + assert o["message"] == "virtual_pin must be contiguous from 0..N-1, got [0, 1, 3]" + + +def test_board_missing_virtual_pin_message_is_explicit_and_mapped(): + s = format_error_line( + "board rp2040_left_arm: missing virtual_pin indices [2, 5] " + "(no actuator mapped to these virtual_pin values)" + ) + o = json.loads(s) + assert o["field"] == "boards.rp2040_left_arm.virtual_pin" + assert o["field_path"] == ["boards", "rp2040_left_arm", "virtual_pin"] + assert "missing virtual_pin indices [2, 5]" in o["message"] + assert "no actuator mapped" in o["message"] diff --git a/lucy_config_pipeline/test/pipeline/test_build.py b/lucy_config_pipeline/test/pipeline/test_build.py new file mode 100644 index 0000000..7ea28a6 --- /dev/null +++ b/lucy_config_pipeline/test/pipeline/test_build.py @@ -0,0 +1,72 @@ +from pathlib import Path + +import pytest + +from src.pipeline import build as pipeline_build + + +def _sample_data() -> dict: + return { + "firmware": {"source_dir": "fw", "build_dir": "build"}, + "boards": {"rp2040_right_arm": {"firmware_target": "pico_micro_ros_right_arm"}}, + } + + +def test_run_build_phase_requires_source_dir(tmp_path: Path): + feedback_calls: list[dict] = [] + with pytest.raises(FileNotFoundError): + pipeline_build.run_build_phase( + data=_sample_data(), + selected_boards=None, + workspace_src=tmp_path, + timeout_seconds=10, + feedback=lambda **kwargs: feedback_calls.append(kwargs), + log_error=lambda _msg: None, + ) + + +def test_run_build_phase_reports_missing_uf2(tmp_path: Path, monkeypatch: pytest.MonkeyPatch): + fw_src = tmp_path / "fw" + fw_src.mkdir(parents=True) + errors: list[str] = [] + + def fake_run_command(**_kwargs): + return None + + monkeypatch.setattr(pipeline_build, "_run_command", fake_run_command) + + failed = pipeline_build.run_build_phase( + data=_sample_data(), + selected_boards=None, + workspace_src=tmp_path, + timeout_seconds=10, + feedback=lambda **_kwargs: None, + log_error=lambda msg: errors.append(msg), + ) + + assert failed == ["rp2040_right_arm"] + assert errors + + +def test_run_build_phase_success(tmp_path: Path, monkeypatch: pytest.MonkeyPatch): + fw_src = tmp_path / "fw" + fw_build = fw_src / "build" + fw_build.mkdir(parents=True) + (fw_build / "CMakeCache.txt").write_text("cached", encoding="utf-8") + + def fake_run_command(*, cmd: list[str], cwd: Path, **_kwargs): + if cmd[0] == "make": + (cwd / "pico_micro_ros_right_arm.uf2").write_text("ok", encoding="utf-8") + + monkeypatch.setattr(pipeline_build, "_run_command", fake_run_command) + + failed = pipeline_build.run_build_phase( + data=_sample_data(), + selected_boards=None, + workspace_src=tmp_path, + timeout_seconds=10, + feedback=lambda **_kwargs: None, + log_error=lambda _msg: None, + ) + + assert failed == [] diff --git a/lucy_config_pipeline/test/pipeline/test_flash.py b/lucy_config_pipeline/test/pipeline/test_flash.py new file mode 100644 index 0000000..de19652 --- /dev/null +++ b/lucy_config_pipeline/test/pipeline/test_flash.py @@ -0,0 +1,163 @@ +from pathlib import Path + +import pytest + +from src.pipeline import flash as pipeline_flash + + +def _sample_data() -> dict: + return { + "firmware": {"source_dir": "fw", "build_dir": "build"}, + "boards": { + "rp2040_right_arm": { + "firmware_target": "pico_micro_ros_right_arm", + "serial_id": "E6617C93E37A6629", + }, + "rp2040_left_arm": { + "firmware_target": "pico_micro_ros_left_arm", + "serial_id": None, + }, + }, + } + + +def test_run_flash_phase_skips_board_without_serial( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +): + monkeypatch.setenv("LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC", "0") + fw_src = tmp_path / "fw" + fw_build = fw_src / "build" + fw_build.mkdir(parents=True) + uf2 = fw_build / "pico_micro_ros_right_arm.uf2" + uf2.write_bytes(b"uf2") + + calls: list[list[str]] = [] + + def fake_run_command(*, cmd: list[str], **_kwargs): + calls.append(cmd) + + monkeypatch.setattr(pipeline_flash, "_run_command", fake_run_command) + monkeypatch.setattr(pipeline_flash, "_wait_for_usb_serial", lambda *_a, **_k: True) + + failed, flashed = pipeline_flash.run_flash_phase( + data=_sample_data(), + selected_boards=None, + boards_built_ok={"rp2040_right_arm", "rp2040_left_arm"}, + workspace_src=tmp_path, + picotool_timeout_seconds=30, + usb_wait_seconds=1, + uptime_wait_seconds=0, + node=None, + feedback=lambda **_kwargs: None, + log_error=lambda _msg: None, + ) + + assert failed == [] + assert flashed == ["rp2040_right_arm"] + assert len(calls) == 1 + assert calls[0][:4] == ["sudo", "picotool", "load", str(uf2)] + assert "E6617C93E37A6629" in calls[0] + + +def test_run_flash_phase_skips_board_not_built_ok(tmp_path: Path, monkeypatch: pytest.MonkeyPatch): + monkeypatch.setenv("LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC", "0") + fw_src = tmp_path / "fw" + fw_build = fw_src / "build" + fw_build.mkdir(parents=True) + (fw_build / "pico_micro_ros_right_arm.uf2").write_bytes(b"uf2") + + calls: list[list[str]] = [] + + def fake_run_command(*, cmd: list[str], **_kwargs): + calls.append(cmd) + + monkeypatch.setattr(pipeline_flash, "_run_command", fake_run_command) + + failed, flashed = pipeline_flash.run_flash_phase( + data=_sample_data(), + selected_boards=None, + boards_built_ok=set(), + workspace_src=tmp_path, + picotool_timeout_seconds=30, + usb_wait_seconds=1, + uptime_wait_seconds=0, + node=None, + feedback=lambda **_kwargs: None, + log_error=lambda _msg: None, + ) + + assert failed == [] + assert flashed == [] + assert calls == [] + + +def test_run_flash_phase_missing_uf2(tmp_path: Path, monkeypatch: pytest.MonkeyPatch): + monkeypatch.setenv("LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC", "0") + fw_src = tmp_path / "fw" + (fw_src / "build").mkdir(parents=True) + + errors: list[str] = [] + + monkeypatch.setattr(pipeline_flash, "_run_command", lambda **_k: None) + + failed, flashed = pipeline_flash.run_flash_phase( + data=_sample_data(), + selected_boards={"rp2040_right_arm"}, + boards_built_ok={"rp2040_right_arm"}, + workspace_src=tmp_path, + picotool_timeout_seconds=30, + usb_wait_seconds=1, + uptime_wait_seconds=0, + node=None, + feedback=lambda **_kwargs: None, + log_error=lambda msg: errors.append(msg), + ) + + assert failed == ["rp2040_right_arm"] + assert flashed == [] + assert errors + + +def test_uptime_topic_from_yaml_and_env(monkeypatch: pytest.MonkeyPatch): + assert pipeline_flash._uptime_topic({"topic_uptime": "ns/uptime"}) == "/ns/uptime" + assert pipeline_flash._uptime_topic({"topic_uptime": "/abs/uptime"}) == "/abs/uptime" + monkeypatch.delenv("LUCY_PIPELINE_UPTIME_TOPIC", raising=False) + assert pipeline_flash._uptime_topic({}) == "/uptime_publisher" + monkeypatch.setenv("LUCY_PIPELINE_UPTIME_TOPIC", "custom/uptime") + assert pipeline_flash._uptime_topic({}) == "/custom/uptime" + + +def test_run_flash_phase_uptime_timeout_fails_board( + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +): + monkeypatch.setenv("LUCY_PIPELINE_FLASH_POST_LOAD_DELAY_SEC", "0") + fw_src = tmp_path / "fw" + fw_build = fw_src / "build" + fw_build.mkdir(parents=True) + uf2 = fw_build / "pico_micro_ros_right_arm.uf2" + uf2.write_bytes(b"uf2") + + monkeypatch.setattr(pipeline_flash, "_run_command", lambda **_k: None) + monkeypatch.setattr(pipeline_flash, "_wait_for_usb_serial", lambda *_a, **_k: True) + monkeypatch.setattr(pipeline_flash, "_wait_uptime_message", lambda *_a, **_k: False) + + class _DummyNode: + pass + + failed, flashed = pipeline_flash.run_flash_phase( + data=_sample_data(), + selected_boards={"rp2040_right_arm"}, + boards_built_ok={"rp2040_right_arm"}, + workspace_src=tmp_path, + picotool_timeout_seconds=30, + usb_wait_seconds=1, + uptime_wait_seconds=5, + node=_DummyNode(), + feedback=lambda **_kwargs: None, + log_error=lambda _msg: None, + ) + + assert failed == ["rp2040_right_arm"] + assert flashed == [] diff --git a/lucy_config_pipeline/test/pipeline/test_selection.py b/lucy_config_pipeline/test/pipeline/test_selection.py new file mode 100644 index 0000000..ea5c7e5 --- /dev/null +++ b/lucy_config_pipeline/test/pipeline/test_selection.py @@ -0,0 +1,85 @@ +from pathlib import Path + +import pytest + +from src.pipeline.selection import ( + board_build_plan, + resolve_firmware_paths, + resolve_mapping_input, + select_boards_to_process, +) +from src.config_store import ConfigStore + + +def test_resolve_mapping_input_active_and_named(tmp_path: Path): + store = ConfigStore(tmp_path / "hardware") + store.ensure_layout() + store.write_named_yaml("default", "version: 1\nrobot_name: t\n") + store.write_named_yaml("alt", "version: 1\nrobot_name: alt\n") + store.activate("default") + + name, txt = resolve_mapping_input(store, "") + assert name == "default" + assert "robot_name: t" in txt + + name, txt = resolve_mapping_input(store, "alt") + assert name == "alt" + assert "robot_name: alt" in txt + + +def test_resolve_mapping_input_from_yaml_path(tmp_path: Path): + store = ConfigStore(tmp_path / "hardware") + store.ensure_layout() + + p = tmp_path / "custom.yaml" + p.write_text("version: 1\nrobot_name: custom\n", encoding="utf-8") + + name, txt = resolve_mapping_input(store, str(p)) + assert name == "custom" + assert "robot_name: custom" in txt + + +def test_select_boards_to_process(): + data = {"boards": {"rp2040_left_arm": {}, "rp2040_right_arm": {}}} + + assert select_boards_to_process(data, []) is None + assert select_boards_to_process(data, ["rp2040_left_arm"]) == {"rp2040_left_arm"} + + with pytest.raises(ValueError, match="unknown boards"): + select_boards_to_process(data, ["rp2040_foo"]) + + +def test_resolve_firmware_paths_relative(tmp_path: Path): + data = {"firmware": {"source_dir": "micro_ros_raspberrypi_pico_sdk", "build_dir": "build"}} + paths = resolve_firmware_paths(data, tmp_path) + assert paths.source_dir == (tmp_path / "micro_ros_raspberrypi_pico_sdk").resolve() + assert paths.build_dir == (tmp_path / "micro_ros_raspberrypi_pico_sdk" / "build").resolve() + + +def test_resolve_firmware_paths_requires_source_dir(tmp_path: Path): + with pytest.raises(ValueError, match="missing firmware.source_dir"): + resolve_firmware_paths({"firmware": {}}, tmp_path) + + +def test_board_build_plan_all_and_selected(): + data = { + "boards": { + "rp2040_left_arm": {"firmware_target": "pico_micro_ros_left_arm"}, + "rp2040_right_arm": {"firmware_target": "pico_micro_ros_right_arm"}, + } + } + + all_plan = board_build_plan(data, None) + assert all_plan == [ + ("rp2040_left_arm", "pico_micro_ros_left_arm"), + ("rp2040_right_arm", "pico_micro_ros_right_arm"), + ] + + selected_plan = board_build_plan(data, {"rp2040_right_arm"}) + assert selected_plan == [("rp2040_right_arm", "pico_micro_ros_right_arm")] + + +def test_board_build_plan_requires_firmware_target(): + data = {"boards": {"rp2040_right_arm": {}}} + with pytest.raises(ValueError, match="missing firmware_target"): + board_build_plan(data, {"rp2040_right_arm"}) diff --git a/lucy_config_pipeline/test/pipeline/test_sim_only.py b/lucy_config_pipeline/test/pipeline/test_sim_only.py new file mode 100644 index 0000000..3d42021 --- /dev/null +++ b/lucy_config_pipeline/test/pipeline/test_sim_only.py @@ -0,0 +1,116 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""ConfigurePipeline simulation_only branch and reload phase (requires ROS env).""" + +from __future__ import annotations + +from pathlib import Path +from unittest.mock import MagicMock, patch + +import pytest + +pytest.importorskip("rclpy") + +import yaml + +from src.pipeline.action_server import PipelineActionServer +from src.pipeline.models import PipelinePaths + +_FIXTURE = ( + Path(__file__).resolve().parents[2] + / ".." + / "lucy_config_generator" + / "test" + / "fixtures" + / "test_mapping.yaml" +) + + +@pytest.fixture +def pipeline_paths(tmp_path: Path) -> PipelinePaths: + robot_root = tmp_path / "thais_urdf" + (robot_root / "description" / "urdf").mkdir(parents=True) + (robot_root / "description" / "ros2_control").mkdir(parents=True) + (robot_root / "config").mkdir(parents=True) + (robot_root / "config" / "hardware").mkdir(parents=True) + urdf_xacro = robot_root / "description" / "urdf" / "inmoov.urdf.xacro" + urdf_xacro.write_text( + (_FIXTURE.parent / "test_robot.urdf.xacro").read_text(encoding="utf-8"), + encoding="utf-8", + ) + (robot_root / "config" / "controllers.yaml").write_text( + "controller_manager:\n ros__parameters:\n update_rate: 100\n", + encoding="utf-8", + ) + return PipelinePaths( + config_dir=robot_root / "config" / "hardware", + urdf_xacro=urdf_xacro, + base_path=robot_root / "description", + controller_config=robot_root / "config" / "controllers.yaml", + robot_root=robot_root, + workspace_src=tmp_path / "src", + ) + + +@pytest.mark.skipif( + not Path("/opt/ros/humble").exists(), + reason="ROS 2 Humble overlay required for lucy_msgs/rclpy", +) +def test_simulation_only_skips_build_flash_and_calls_reload(pipeline_paths: PipelinePaths): + data = yaml.safe_load(_FIXTURE.read_text(encoding="utf-8")) + config_yaml = yaml.dump(data) + + store = MagicMock() + store.get_active_name.return_value = "default" + + node = PipelineActionServer(paths=pipeline_paths, config_store=store) + node._reload_client.wait_for_service = MagicMock(return_value=True) # type: ignore[method-assign] + + future = MagicMock() + future.done.return_value = True + resp = MagicMock() + resp.success = True + resp.message = "ok" + future.result.return_value = resp + # The new wait path uses future.add_done_callback + threading.Event, + # not rclpy.spin_until_future_complete. Make the callback fire immediately + # so done_event is set before .wait() is reached. + future.add_done_callback.side_effect = lambda cb: cb(future) + node._reload_client.call_async = MagicMock(return_value=future) # type: ignore[method-assign] + + goal_handle = MagicMock() + goal_handle.request.mapping_file = "" + goal_handle.request.boards_to_flash = [] + goal_handle.request.dry_run = False + goal_handle.request.build_only = False + goal_handle.request.simulation_only = True + goal_handle.is_cancel_requested = False + + with ( + patch("src.pipeline.action_server.resolve_mapping_input", return_value=("default", config_yaml)), + patch("src.pipeline.action_server.validate_schema", return_value=data), + patch("src.pipeline.action_server.urdf_crosscheck") as cross, + patch("src.pipeline.action_server.generate") as gen, + patch("src.pipeline.action_server.run_build_phase") as build, + patch("src.pipeline.action_server.run_flash_phase") as flash, + ): + cross.return_value = MagicMock(errors=[]) + gen.side_effect = lambda **kwargs: _write_ros2_outputs(kwargs["output_dir"]) + + result = node._execute(goal_handle) + + assert result.success is True + build.assert_not_called() + flash.assert_not_called() + assert any(c.kwargs.get("simulation_only") is True for c in gen.call_args_list) + node._reload_client.call_async.assert_called_once() + + +def _write_ros2_outputs(out_dir: Path) -> None: + (out_dir / "inmoov_ros2_control.xacro").write_text("", encoding="utf-8") + (out_dir / "controllers.yaml").write_text( + "controller_manager:\n ros__parameters:\n update_rate: 100\n", + encoding="utf-8", + ) diff --git a/lucy_config_pipeline/test/services/test_config_services_node.py b/lucy_config_pipeline/test/services/test_config_services_node.py new file mode 100644 index 0000000..69dd2ad --- /dev/null +++ b/lucy_config_pipeline/test/services/test_config_services_node.py @@ -0,0 +1,34 @@ +from pathlib import Path + +import pytest +import rclpy + +from src.config_store import ConfigStore +from src.services.config_services_node import ConfigServicesNode + + +@pytest.fixture +def rclpy_init_shutdown(): + rclpy.init() + yield + rclpy.shutdown() + + +class TestConfigServicesNode: + def test_get_client_count_service(self, rclpy_init_shutdown, tmp_path: Path): + """Test /get_client_count service returns the current client count.""" + from lucy_msgs.srv import GetInt + + node = ConfigServicesNode( + robot_package="test_pkg", + config_store=ConfigStore(tmp_path / "hardware"), + urdf_xacro=Path("/dev/null"), + base_path=tmp_path, + controller_config=Path("/dev/null"), + ) + node._client_count = 5 + + response = node._on_get_client_count(GetInt.Request(), GetInt.Response()) + + assert response.value == 5 + node.destroy_node() diff --git a/lucy_config_pipeline/test/store/test_config_store.py b/lucy_config_pipeline/test/store/test_config_store.py new file mode 100644 index 0000000..ced279e --- /dev/null +++ b/lucy_config_pipeline/test/store/test_config_store.py @@ -0,0 +1,44 @@ +from pathlib import Path + +from src.config_store import ConfigStore + + +def test_save_list_activate_delete(tmp_path: Path): + store = ConfigStore(tmp_path / "hardware") + store.ensure_layout() + + a = "version: 1\nrobot_name: t\n" + b = "version: 1\nrobot_name: t2\n" + store.write_named_yaml("default", a) + store.write_named_yaml("alt", b) + + assert store.list_configs() == ["alt", "default"] + + backup = store.activate("default") + assert backup == "" + assert store.get_active_name() == "default" + + backup = store.activate("alt") + assert backup != "" + assert (store.backups_dir / f"{backup}.yaml").is_file() + + try: + store.delete("default") + assert False, "default deletion should fail" + except ValueError: + pass + + +def test_activate_preserves_flashed_meta(tmp_path: Path): + store = ConfigStore(tmp_path / "hardware") + store.ensure_layout() + store.write_named_yaml("a", "version: 1\nrobot_name: x\n") + store.activate("a") + store.record_flashed_preset("a") + assert store.get_flashed_name() == "a" + assert store.get_flashed_at() + + store.write_named_yaml("b", "version: 1\nrobot_name: y\n") + store.activate("b") + assert store.get_active_name() == "b" + assert store.get_flashed_name() == "a" diff --git a/lucy_config_pipeline/test/test_copyright.py b/lucy_config_pipeline/test/test_copyright.py new file mode 100644 index 0000000..82dca60 --- /dev/null +++ b/lucy_config_pipeline/test/test_copyright.py @@ -0,0 +1,11 @@ +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header. +@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.") +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found errors" diff --git a/lucy_config_pipeline/test/test_flake8.py b/lucy_config_pipeline/test/test_flake8.py new file mode 100644 index 0000000..f2bd7bc --- /dev/null +++ b/lucy_config_pipeline/test/test_flake8.py @@ -0,0 +1,27 @@ +import warnings +from pathlib import Path + +import pytest +from ament_flake8.main import main_with_errors + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + pkg_root = Path(__file__).resolve().parents[1] + targets = [ + str(pkg_root / "setup.py"), + str(pkg_root / "src"), + str(pkg_root / "test"), + ] + with warnings.catch_warnings(): + warnings.filterwarnings( + "ignore", + message="SelectableGroups dict interface is deprecated", + category=DeprecationWarning, + ) + rc, errors = main_with_errors(argv=targets) + assert rc == 0, ( + "Found %d code style errors / warnings:\n" % len(errors) + + "\n".join(errors) + ) diff --git a/lucy_config_pipeline/test/test_pep257.py b/lucy_config_pipeline/test/test_pep257.py new file mode 100644 index 0000000..da93c14 --- /dev/null +++ b/lucy_config_pipeline/test/test_pep257.py @@ -0,0 +1,9 @@ +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=[".", "test"]) + assert rc == 0, "Found code style errors / warnings" diff --git a/lucy_config_pipeline/test/validators/test_checks.py b/lucy_config_pipeline/test/validators/test_checks.py new file mode 100644 index 0000000..1340b8f --- /dev/null +++ b/lucy_config_pipeline/test/validators/test_checks.py @@ -0,0 +1,20 @@ +from src.validation import ValidationReport, parse_yaml_text + + +def test_parse_yaml_mapping(): + data = parse_yaml_text("version: 1\nrobot_name: inmoov\n") + assert data["version"] == 1 + + +def test_parse_yaml_rejects_non_mapping(): + try: + parse_yaml_text("- 1\n- 2\n") + assert False, "list root must fail" + except ValueError: + pass + + +def test_validation_report_dataclass(): + r = ValidationReport(errors=["e"], warnings=["w"]) + assert r.errors == ["e"] + assert r.warnings == ["w"] diff --git a/lucy_control_supervisor/launch/control_supervisor.launch.py b/lucy_control_supervisor/launch/control_supervisor.launch.py new file mode 100644 index 0000000..204f49b --- /dev/null +++ b/lucy_control_supervisor/launch/control_supervisor.launch.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Start lucy_control_supervisor (RSP + ros2_control + spawners, /lucy_control/restart).""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("urdf_path", default_value=""), + DeclareLaunchArgument("base_path", default_value=""), + DeclareLaunchArgument("controllers_yaml", default_value=""), + DeclareLaunchArgument("use_gazebo_sim", default_value="false"), + DeclareLaunchArgument("use_mock_hardware", default_value="false"), + DeclareLaunchArgument("gazebo_only", default_value="false"), + DeclareLaunchArgument("autostart", default_value="true"), + DeclareLaunchArgument( + "ros2_control_file", default_value="inmoov_ros2_control.xacro" + ), + Node( + package="lucy_control_supervisor", + executable="control_supervisor_node", + name="lucy_control_supervisor", + output="screen", + parameters=[ + { + "urdf_path": LaunchConfiguration("urdf_path"), + "base_path": LaunchConfiguration("base_path"), + "controllers_yaml": LaunchConfiguration("controllers_yaml"), + "use_gazebo_sim": LaunchConfiguration("use_gazebo_sim"), + "use_mock_hardware": LaunchConfiguration("use_mock_hardware"), + "gazebo_only": LaunchConfiguration("gazebo_only"), + "autostart": LaunchConfiguration("autostart"), + "ros2_control_file": LaunchConfiguration("ros2_control_file"), + } + ], + ), + ] + ) diff --git a/lucy_control_supervisor/lucy_control_supervisor/__init__.py b/lucy_control_supervisor/lucy_control_supervisor/__init__.py new file mode 100644 index 0000000..d4d8e08 --- /dev/null +++ b/lucy_control_supervisor/lucy_control_supervisor/__init__.py @@ -0,0 +1 @@ +# Lucy control stack supervisor package. diff --git a/lucy_control_supervisor/lucy_control_supervisor/controllers_spawn.py b/lucy_control_supervisor/lucy_control_supervisor/controllers_spawn.py new file mode 100644 index 0000000..4a68501 --- /dev/null +++ b/lucy_control_supervisor/lucy_control_supervisor/controllers_spawn.py @@ -0,0 +1,21 @@ +"""Shared helper: controller names declared in controllers.yaml.""" + +from __future__ import annotations + +from pathlib import Path + +import yaml + + +def controllers_to_spawn(controllers_yaml_path: Path) -> list[str]: + """Return controller names under controller_manager.ros__parameters (except update_rate).""" + data = yaml.safe_load(controllers_yaml_path.read_text(encoding="utf-8")) or {} + cm_params = data.get("controller_manager", {}).get("ros__parameters", {}) + if not isinstance(cm_params, dict): + return [] + names = [name for name in cm_params.keys() if name != "update_rate"] + jsb = "joint_state_broadcaster" + if jsb in names: + names.remove(jsb) + return [jsb, *names] + return names diff --git a/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py b/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py new file mode 100644 index 0000000..d863499 --- /dev/null +++ b/lucy_control_supervisor/lucy_control_supervisor/supervisor_node.py @@ -0,0 +1,280 @@ +# Copyright 2025 Sentience Robotics Team +# +# SPDX-License-Identifier: GPL-3.0-only + +"""Manage RSP + ros2_control_node + spawners; restart on /lucy_control/restart.""" + +from __future__ import annotations + +import os +import shutil +import signal +import subprocess +import tempfile +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import List, Optional + +import rclpy +import yaml +from lucy_control_supervisor.controllers_spawn import controllers_to_spawn +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import Bool +from std_srvs.srv import Trigger + + +@dataclass +class _ManagedProc: + name: str + popen: subprocess.Popen + kind: str # rsp | cm | spawner + + +@dataclass +class _StackConfig: + urdf_path: Path + base_path: Path + controllers_yaml: Path + use_gazebo_sim: bool + use_mock_hardware: bool + gazebo_only: bool + ros2_control_file: str + + +class ControlSupervisorNode(Node): + RESTART_SERVICE = "/lucy_control/restart" + GAZEBO_RUNNING_TOPIC = "/lucy/gazebo_running" + TERMINATE_TIMEOUT_S = 10.0 + SPAWN_SETTLE_S = 2.0 + + def __init__(self) -> None: + super().__init__("lucy_control_supervisor") + self.declare_parameter("urdf_path", "") + self.declare_parameter("base_path", "") + self.declare_parameter("controllers_yaml", "") + self.declare_parameter("use_gazebo_sim", False) + self.declare_parameter("use_mock_hardware", False) + self.declare_parameter("gazebo_only", False) + self.declare_parameter("autostart", True) + self.declare_parameter("ros2_control_file", "inmoov_ros2_control.xacro") + + self._children: List[_ManagedProc] = [] + self._restart_lock = False + self._srv = self.create_service(Trigger, self.RESTART_SERVICE, self._on_restart) + # Latched publisher so any late subscriber (e.g. the LCP after rosbridge reconnects) + # immediately sees whether Gazebo is part of the current launch. + latched_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + self._gazebo_running_pub = self.create_publisher( + Bool, self.GAZEBO_RUNNING_TOPIC, latched_qos + ) + self._publish_gazebo_running() + if self.get_parameter("autostart").value: + ok, msg = self._restart_stack() + if not ok: + self.get_logger().error(msg) + + def _publish_gazebo_running(self) -> None: + msg = Bool() + msg.data = bool(self.get_parameter("use_gazebo_sim").value) + self._gazebo_running_pub.publish(msg) + + def destroy_node(self) -> bool: + self._terminate_children() + return super().destroy_node() + + def _cfg(self) -> _StackConfig: + return _StackConfig( + urdf_path=Path(self.get_parameter("urdf_path").value).resolve(), + base_path=Path(self.get_parameter("base_path").value).resolve(), + controllers_yaml=Path(self.get_parameter("controllers_yaml").value).resolve(), + use_gazebo_sim=bool(self.get_parameter("use_gazebo_sim").value), + use_mock_hardware=bool(self.get_parameter("use_mock_hardware").value), + gazebo_only=bool(self.get_parameter("gazebo_only").value), + ros2_control_file=str( + self.get_parameter("ros2_control_file").value or "inmoov_ros2_control.xacro" + ).strip(), + ) + + def _xacro_cmd(self, cfg: _StackConfig) -> list[str]: + tail = [ + str(cfg.urdf_path), + f"base_path:={cfg.base_path}", + f"controller_config:={cfg.controllers_yaml}", + f"use_gazebo_sim:={'true' if cfg.use_gazebo_sim else 'false'}", + f"use_mock_hardware:={'true' if cfg.use_mock_hardware else 'false'}", + f"ros2_control_file:={cfg.ros2_control_file}", + ] + if shutil.which("ros2"): + return ["ros2", "run", "xacro", "xacro", *tail] + if shutil.which("xacro"): + return ["xacro", *tail] + raise RuntimeError("xacro not found on PATH") + + def _expand_robot_description(self, cfg: _StackConfig) -> str: + cmd = self._xacro_cmd(cfg) + r = subprocess.run(cmd, capture_output=True, text=True, timeout=120, check=False) + if r.returncode != 0: + raise RuntimeError(f"xacro failed: {r.stderr or r.stdout}") + return r.stdout + + def _terminate_children(self) -> None: + for child in reversed(self._children): + if child.popen.poll() is None: + try: + child.popen.send_signal(signal.SIGTERM) + except ProcessLookupError: + pass + deadline = time.monotonic() + self.TERMINATE_TIMEOUT_S + for child in self._children: + while child.popen.poll() is None and time.monotonic() < deadline: + time.sleep(0.1) + for child in self._children: + if child.popen.poll() is None: + try: + child.popen.kill() + except ProcessLookupError: + pass + self._children.clear() + + def _start_rsp(self, cfg: _StackConfig, urdf_xml: str) -> None: + payload = { + "robot_state_publisher": { + "ros__parameters": { + "robot_description": urdf_xml, + "use_sim_time": bool(cfg.use_gazebo_sim), + } + } + } + with tempfile.NamedTemporaryFile( + mode="w", suffix=".yaml", delete=False, encoding="utf-8" + ) as f: + yaml.safe_dump(payload, f, sort_keys=False) + params_file = f.name + cmd = [ + "ros2", + "run", + "robot_state_publisher", + "robot_state_publisher", + "--ros-args", + "--params-file", + params_file, + ] + popen = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + env=os.environ.copy(), + ) + self._children.append(_ManagedProc("robot_state_publisher", popen, "rsp")) + time.sleep(self.SPAWN_SETTLE_S) + + def _start_cm(self, cfg: _StackConfig) -> None: + cmd = [ + "ros2", + "run", + "controller_manager", + "ros2_control_node", + "--ros-args", + "--params-file", + str(cfg.controllers_yaml), + "-r", + "~/robot_description:=/robot_description", + ] + popen = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + env=os.environ.copy(), + ) + self._children.append(_ManagedProc("ros2_control_node", popen, "cm")) + time.sleep(self.SPAWN_SETTLE_S) + + def _start_spawners(self, cfg: _StackConfig) -> Optional[str]: + names = controllers_to_spawn(cfg.controllers_yaml) + if not names: + return "no controllers found in controllers.yaml" + for name in names: + cmd = [ + "ros2", + "run", + "controller_manager", + "spawner", + name, + "--switch-timeout", + "10", + ] + if cfg.use_gazebo_sim: + cmd.extend(["--ros-args", "-p", "use_sim_time:=true"]) + popen = subprocess.Popen( + cmd, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + env=os.environ.copy(), + ) + self._children.append(_ManagedProc(f"spawner_{name}", popen, "spawner")) + time.sleep(1.0) + return None + + def _restart_stack(self) -> tuple[bool, str]: + if self._restart_lock: + return False, "restart already in progress" + self._restart_lock = True + try: + cfg = self._cfg() + for p in (cfg.urdf_path, cfg.base_path, cfg.controllers_yaml): + if not p.exists(): + return False, f"missing path: {p}" + + self._terminate_children() + urdf_xml = self._expand_robot_description(cfg) + + if not cfg.gazebo_only: + self._start_rsp(cfg, urdf_xml) + if not cfg.use_gazebo_sim: + self._start_cm(cfg) + + err = self._start_spawners(cfg) + if err: + return False, err + + note = "" + if cfg.use_gazebo_sim: + note = ( + " Gazebo: spawners restarted; if URDF hardware topology changed, " + "restart Gazebo (gz_ros2_control loads URDF at spawn)." + ) + return True, f"control stack restarted.{note}" + except Exception as e: + return False, str(e) + finally: + self._restart_lock = False + + def _on_restart(self, _request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: + ok, msg = self._restart_stack() + response.success = ok + response.message = msg + if not ok: + self.get_logger().error(msg) + else: + self.get_logger().info(msg) + return response + + +def main() -> None: + rclpy.init() + node = ControlSupervisorNode() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/lucy_control_supervisor/package.xml b/lucy_control_supervisor/package.xml new file mode 100644 index 0000000..f453aaf --- /dev/null +++ b/lucy_control_supervisor/package.xml @@ -0,0 +1,26 @@ + + + + lucy_control_supervisor + 0.0.0 + Supervises ros2_control stack restart for LCP-driven configuration reload. + Sentience Robotics Team + GPL-3.0 + + rclpy + std_msgs + std_srvs + ament_index_python + robot_state_publisher + controller_manager + xacro + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/lucy_control_supervisor/resource/lucy_control_supervisor b/lucy_control_supervisor/resource/lucy_control_supervisor new file mode 100644 index 0000000..e69de29 diff --git a/lucy_control_supervisor/setup.cfg b/lucy_control_supervisor/setup.cfg new file mode 100644 index 0000000..e6269a3 --- /dev/null +++ b/lucy_control_supervisor/setup.cfg @@ -0,0 +1,7 @@ +[develop] +script_dir=$base/lib/lucy_control_supervisor +[install] +install_scripts=$base/lib/lucy_control_supervisor + +[flake8] +max-line-length = 100 diff --git a/lucy_control_supervisor/setup.py b/lucy_control_supervisor/setup.py new file mode 100644 index 0000000..933d53a --- /dev/null +++ b/lucy_control_supervisor/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = "lucy_control_supervisor" + +setup( + name=package_name, + version="0.0.0", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", ["launch/control_supervisor.launch.py"]), + ], + install_requires=["setuptools", "PyYAML"], + zip_safe=True, + maintainer="Sentience Robotics Team", + maintainer_email="contact@sentience-robotics.fr", + description="Restart RSP + ros2_control_node + spawners after config pipeline reload.", + license="GPL-3.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "control_supervisor_node = lucy_control_supervisor.supervisor_node:main", + ], + }, +) diff --git a/lucy_msgs/CMakeLists.txt b/lucy_msgs/CMakeLists.txt new file mode 100644 index 0000000..e687ded --- /dev/null +++ b/lucy_msgs/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(lucy_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(action_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/RawSensor.msg" + "action/ConfigurePipeline.action" + "srv/GetInt.srv" + "srv/ListConfigs.srv" + "srv/GetConfig.srv" + "srv/GetMesh.srv" + "srv/SaveConfig.srv" + "srv/ActivateConfig.srv" + "srv/DeleteConfig.srv" + DEPENDENCIES action_msgs builtin_interfaces +) + +ament_package() diff --git a/lucy_msgs/action/ConfigurePipeline.action b/lucy_msgs/action/ConfigurePipeline.action new file mode 100644 index 0000000..ff03a18 --- /dev/null +++ b/lucy_msgs/action/ConfigurePipeline.action @@ -0,0 +1,20 @@ +# Goal: trigger the hardware configuration pipeline +string robot_package # e.g. "inmoov_urdf" +string mapping_file # config name or path (empty = active config) +string[] boards_to_flash # board names to flash (empty = all boards) +bool dry_run # validate + generate only, no build/flash +bool build_only # validate + generate + build, no flash +bool simulation_only # skip build/flash; generate sim ros2_control + reload +--- +# Result +bool success +string message +string config_name # name of the config that was applied +string[] boards_flashed # board names that were successfully flashed +string[] errors # per-phase error messages (if any) +--- +# Feedback +string phase # "validate", "generate", "build", "flash", "reload" +string board # current board being processed (empty if N/A) +float64 progress # 0.0 to 1.0 within the current phase (float64 avoids 0.1 float32 echo noise) +string detail # human-readable status message diff --git a/lucy_msgs/msg/RawSensor.msg b/lucy_msgs/msg/RawSensor.msg new file mode 100644 index 0000000..77f4d93 --- /dev/null +++ b/lucy_msgs/msg/RawSensor.msg @@ -0,0 +1 @@ +float32[] values diff --git a/lucy_msgs/package.xml b/lucy_msgs/package.xml new file mode 100644 index 0000000..6f506a2 --- /dev/null +++ b/lucy_msgs/package.xml @@ -0,0 +1,20 @@ + + + + lucy_msgs + 0.1.0 + ROS2 interface definitions for Lucy robot: messages, actions, and services + Sentience Robotics Team + GPL-3.0 + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + action_msgs + builtin_interfaces + rosidl_interface_packages + + + ament_cmake + + diff --git a/lucy_msgs/srv/ActivateConfig.srv b/lucy_msgs/srv/ActivateConfig.srv new file mode 100644 index 0000000..5c9df2d --- /dev/null +++ b/lucy_msgs/srv/ActivateConfig.srv @@ -0,0 +1,8 @@ +# Request +string robot_package # e.g. "inmoov_urdf" +string config_name # name of config to activate +--- +# Response +bool success +string message +string backup_name # name of the backup created from the previous active config diff --git a/lucy_msgs/srv/DeleteConfig.srv b/lucy_msgs/srv/DeleteConfig.srv new file mode 100644 index 0000000..cefd0b7 --- /dev/null +++ b/lucy_msgs/srv/DeleteConfig.srv @@ -0,0 +1,7 @@ +# Request +string robot_package # e.g. "inmoov_urdf" +string config_name # name of config to delete +--- +# Response +bool success +string message diff --git a/lucy_msgs/srv/GetConfig.srv b/lucy_msgs/srv/GetConfig.srv new file mode 100644 index 0000000..80dc804 --- /dev/null +++ b/lucy_msgs/srv/GetConfig.srv @@ -0,0 +1,13 @@ +# Request +string robot_package # e.g. "inmoov_urdf" +string config_name # name of config to retrieve (empty = active) +--- +# Response +bool success +string message +string robot_package # ROS package name wired on `lucy_config_services` (e.g. inmoov_urdf) +string config_name # resolved config name +string config_yaml # full YAML content as string +# Last preset successfully written to firmware (active_meta.yaml); empty if never flashed via pipeline +string flashed_config_name +string flashed_at # ISO-8601 UTC timestamp; empty if unknown / never flashed diff --git a/camera_ros/srv/GetInt.srv b/lucy_msgs/srv/GetInt.srv similarity index 100% rename from camera_ros/srv/GetInt.srv rename to lucy_msgs/srv/GetInt.srv diff --git a/lucy_msgs/srv/GetMesh.srv b/lucy_msgs/srv/GetMesh.srv new file mode 100644 index 0000000..70d62b3 --- /dev/null +++ b/lucy_msgs/srv/GetMesh.srv @@ -0,0 +1,7 @@ +# Request +string path # mesh reference from the URDF (file://, package://, or absolute) +--- +# Response +bool success +string message +string data # mesh file contents as text (e.g. Collada/DAE XML) diff --git a/lucy_msgs/srv/ListConfigs.srv b/lucy_msgs/srv/ListConfigs.srv new file mode 100644 index 0000000..81360b4 --- /dev/null +++ b/lucy_msgs/srv/ListConfigs.srv @@ -0,0 +1,8 @@ +# Request +string robot_package # e.g. "inmoov_urdf" +--- +# Response +bool success +string message +string active_config # name of the currently active config +string[] config_names # all available config names diff --git a/lucy_msgs/srv/SaveConfig.srv b/lucy_msgs/srv/SaveConfig.srv new file mode 100644 index 0000000..75174e3 --- /dev/null +++ b/lucy_msgs/srv/SaveConfig.srv @@ -0,0 +1,11 @@ +# Request +string robot_package # e.g. "inmoov_urdf" +string config_name # name to save as +string config_yaml # full YAML content as string +bool activate # also activate this config after saving +--- +# Response +bool success +string message +string[] validation_errors # blocking schema/consistency errors +string[] urdf_warnings # non-blocking URDF cross-check warnings diff --git a/lucy_ros2_control/CMakeLists.txt b/lucy_ros2_control/CMakeLists.txt index eb3ede8..5e28cdb 100644 --- a/lucy_ros2_control/CMakeLists.txt +++ b/lucy_ros2_control/CMakeLists.txt @@ -24,12 +24,13 @@ endforeach() add_library( lucy_ros2_control SHARED - hardware/lucy_system.cpp + src/lucy_system.cpp + src/joint_config.cpp ) target_compile_features(lucy_ros2_control PUBLIC cxx_std_17) target_include_directories(lucy_ros2_control PUBLIC -$ +$ $ ) ament_target_dependencies( @@ -43,22 +44,17 @@ pluginlib_export_plugin_description_file(hardware_interface lucy_ros2_control.xm # Installing install( - DIRECTORY hardware/include/ + DIRECTORY src/include/ DESTINATION include/lucy_ros2_control ) -install( - DIRECTORY config launch - DESTINATION share/${PROJECT_NAME}/ -) - # install( # DIRECTORY description/launch description/ros2_control description/urdf -# DESTINATION share/ros2_control_demo_example_2 +# DESTINATION share/lucy_ros2_control # ) # install( # DIRECTORY bringup/launch bringup/config -# DESTINATION share/ros2_control_demo_example_2 +# DESTINATION share/lucy_ros2_control # ) install(TARGETS lucy_ros2_control EXPORT export_lucy_ros2_control @@ -68,17 +64,30 @@ install(TARGETS lucy_ros2_control ) if(BUILD_TESTING) - # Apache-2.0 headers in hardware/*.cpp vs GPL-3.0 in package.xml; cpplint noise on vendored style. + # cpplint excluded: noise on the inherited ros2_control style; copyright lint + # expects the standard GPLv3 header block (matches lucy_bringup / package.xml). set(AMENT_LINT_AUTO_EXCLUDE - ament_cmake_copyright ament_cmake_cpplint ) - find_package(ament_cmake_pytest REQUIRED) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_pytest_test(test_controllers_yaml test/test_controllers_yaml.py - TIMEOUT 60 + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_position_limit_clamp test/test_position_limit_clamp.cpp) + target_include_directories(test_position_limit_clamp PRIVATE + ${PROJECT_SOURCE_DIR}/src/include + ) + + # Pure on_init helpers: compile the module straight into the test (no ROS node + # needed); depends on hardware_interface for ComponentInfo / lexical_casts. + ament_add_gtest(test_joint_config + test/test_joint_config.cpp + src/joint_config.cpp + ) + target_include_directories(test_joint_config PRIVATE + ${PROJECT_SOURCE_DIR}/src/include ) + ament_target_dependencies(test_joint_config hardware_interface) endif() ament_package() diff --git a/lucy_ros2_control/README.md b/lucy_ros2_control/README.md index 0dafa56..4d5d1a3 100644 --- a/lucy_ros2_control/README.md +++ b/lucy_ros2_control/README.md @@ -1,14 +1,13 @@ # lucy_ros2_control -ROS 2 Control for Lucy: hardware interface, controller config, and launch file. +ROS 2 Control hardware plugin for Lucy-compatible robots. -**Architecture (repo-level):** [`../doc/ROS2_CONTROL.md`](../doc/ROS2_CONTROL.md) — how ros2_control maps to Lucy, `/actuators/*`, launches, and pitfalls. +**Architecture (repo-level):** [`../docs/ROS2_CONTROL.md`](../docs/ROS2_CONTROL.md) — how ros2_control maps to Lucy, `/actuators/*`, launches, and pitfalls. ## Contents -- **Hardware interface** — `LucySystemHardware` (publishes `/actuators/left_arm`, `/actuators/right_arm` for micro-controllers). -- **Config** — `config/lucy_controllers.yaml`. -- **Launch** — `control.launch.py`: robot_state_publisher + ros2_control_node + spawners. +- **Hardware interface plugin** — `LucySystemHardware` in `src/lucy_system.cpp`. +- **Plugin description** — `lucy_ros2_control.xml`. ## Building @@ -20,20 +19,38 @@ colcon build --symlink-install --packages-select lucy_ros2_control source install/setup.bash ``` -## Quick start +## Hardware parameters -```bash -ros2 launch lucy_ros2_control control.launch.py -``` +`LucySystemHardware` reads joint parameters from ros2_control `HardwareInfo` (the +`` tags in robot-specific `inmoov_ros2_control.xacro` generated from YAML): + +- `virtual_pin` +- `offset_deg` +- `direction` +- `scale` +- `servo_min_deg` +- `servo_max_deg` +- `servo_default_deg` + +Joints without `virtual_pin` are treated as passive/unmapped for actuator output. + +## Conversion math -Optional args: `urdf_path:=` `base_path:=` (defaults: **`thais_urdf`** package share → `inmoov/`). Requires **`thais_urdf`** installed in the same workspace / underlay. +Internal command/state interface uses joint-space radians. Published micro-ROS +command uses servo-space radians by virtual pin. -## Real + RViz + rosbridge / Gazebo + RViz + rosbridge +- default initialization: + - `joint_rad = deg_to_rad((servo_default_deg - offset_deg) * direction * scale)` +- write conversion: + - `servo_deg = rad_to_deg(joint_rad) / (direction * scale) + offset_deg` + - `servo_deg` clamped to `[servo_min_deg, servo_max_deg]` + - publish `deg_to_rad(servo_deg)` to `position[virtual_pin]` -Use the **thais_urdf** package: +## Robot-specific launch/config -- `ros2 launch thais_urdf rviz.launch.py` — real robot + RViz + rosbridge (control panel at ws://localhost:9090). -- `ros2 launch thais_urdf gazebo.launch.py` — Gazebo sim + RViz + ros2_control (sim) + rosbridge. +This package no longer installs robot-specific launch files or controller YAML. +Those belong in robot description packages (e.g. `thais_urdf`) and should be +generated by `lucy_config_generator`. ## Dependencies diff --git a/lucy_ros2_control/config/lucy_controllers.yaml b/lucy_ros2_control/config/lucy_controllers.yaml deleted file mode 100644 index 66bac02..0000000 --- a/lucy_ros2_control/config/lucy_controllers.yaml +++ /dev/null @@ -1,92 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 100 - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - left_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - right_arm_controller: - type: joint_trajectory_controller/JointTrajectoryController - -# Extra joints (not in ros2_control) published at 0 so robot_state_publisher builds full TF tree. -# Fixes disconnected tree in view_frames (base_node->stand_link vs torso_y_link->arms). -joint_state_broadcaster: - ros__parameters: - extra_joints: - - Empty.001_link_joint - - Empty.002_link_joint - - i01.torso.midStom_link_joint - - i01.torso.topStom_link_joint - - left_shoulder_z_link_joint - - i01.leftHand.wrist.001_link_joint - - i01.leftHand.index2_link_joint - - i01.leftHand.index3_link_joint - - i01.leftHand.majeure2_link_joint - - i01.leftHand.majeure3_link_joint - - i01.leftHand.pinky0_link_joint - - i01.leftHand.pinky2_link_joint - - i01.leftHand.pinky3_link_joint - - i01.leftHand.ringfinger0_link_joint - - i01.leftHand.ringfinger2_link_joint - - i01.leftHand.ringfinger3_link_joint - - i01.leftHand.thumb1_link_joint - - i01.leftHand.thumb3_link_joint - - right_shoulder_z_link_joint - - i01.rightHand.wrist.001_link_joint - - i01.rightHand.index2_link_joint - - i01.rightHand.index3_link_joint - - i01.rightHand.majeure2_link_joint - - i01.rightHand.majeure3_link_joint - - i01.rightHand.pinky0_link_joint - - i01.rightHand.pinky2_link_joint - - i01.rightHand.pinky3_link_joint - - i01.rightHand.ringfinger0_link_joint - - i01.rightHand.ringfinger2_link_joint - - i01.rightHand.ringfinger3_link_joint - - i01.rightHand.thumb1_link_joint - - i01.rightHand.thumb3_link_joint - - i01.head.neck.001_link_joint - - i01.head.rollNeck_link_joint - - i01.head.rothead_link_joint - - i01.head.jaw_link_joint - - i01.head.eyeRight_link_joint - - i01.head.eyeRight.001_link_joint - - i01.head.eyeLeft.001_link_joint - - i01.head.eyeLeft_link_joint - -# Joint names must match URDF exactly for correct TF tree (robot_state_publisher + RViz/Gazebo) -left_arm_controller: - ros__parameters: - joints: - - left_shoulder_y_link_joint - - left_shoulder_z_link_joint - - left_shoulder_x_link_joint - - left_elbow_x_link_joint - - left_wrist_z_link_joint - - i01.leftHand.thumb_link_joint - - i01.leftHand.index_link_joint - - i01.leftHand.majeure_link_joint - - i01.leftHand.ringFinger_link_joint - - i01.leftHand.pinky_link_joint - command_interfaces: - - position - state_interfaces: - - position - -right_arm_controller: - ros__parameters: - joints: - - right_shoulder_y_link_joint - - right_shoulder_z_link_joint - - right_shoulder_x_link_joint - - right_elbow - - right_wrist_z_link_joint - - i01.rightHand.thumb_link_joint - - i01.rightHand.index_link_joint - - i01.rightHand.majeure_link_joint - - i01.rightHand.ringFinger_link_joint - - i01.rightHand.pinky_link_joint - command_interfaces: - - position - state_interfaces: - - position \ No newline at end of file diff --git a/lucy_ros2_control/hardware/include/lucy_system.hpp b/lucy_ros2_control/hardware/include/lucy_system.hpp deleted file mode 100644 index 36cdc15..0000000 --- a/lucy_ros2_control/hardware/include/lucy_system.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright 2021 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ -#define ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ - -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/publisher.hpp" -#include "rclcpp/clock.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include - -namespace ros2_control_demo_example_2 -{ -class LucySystemHardware : public hardware_interface::SystemInterface -{ -public: - RCLCPP_SHARED_PTR_DEFINITIONS(LucySystemHardware) - - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareInfo & info) override; - - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; - - hardware_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State & previous_state) override; - - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; - - rclcpp::Logger get_logger() const {return *logger_;} - // rclcpp::Clock::SharedPtr get_clock() const { return clock_; } - -private: - rclcpp::Publisher::SharedPtr joint_publisher_; - rclcpp::Node::SharedPtr node_; - - // Objects for logging - std::shared_ptr logger_; - // rclcpp::Clock::SharedPtr clock_; - - // Store the command for the simulated robot - std::vector hw_commands_; - std::vector hw_positions_; - // std::vector hw_velocities_; // We have no velocity for our servos -}; - -} // namespace ros2_control_demo_example_2 - -#endif // ROS2_CONTROL_DEMO_EXAMPLE_2__DIFFBOT_SYSTEM_HPP_ diff --git a/lucy_ros2_control/hardware/lucy_system.cpp b/lucy_ros2_control/hardware/lucy_system.cpp deleted file mode 100644 index 9624e28..0000000 --- a/lucy_ros2_control/hardware/lucy_system.cpp +++ /dev/null @@ -1,194 +0,0 @@ -// Copyright 2021 ros2_control Development Team -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "include/lucy_system.hpp" - -#include -#include -#include -#include -#include -#include - -#include "hardware_interface/lexical_casts.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/qos.hpp" - -namespace ros2_control_demo_example_2 -{ -hardware_interface::CallbackReturn LucySystemHardware::on_init( - const hardware_interface::HardwareInfo & info) -{ - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { - return hardware_interface::CallbackReturn::ERROR; - } - - logger_ = std::make_shared( - rclcpp::get_logger((info_.name).c_str())); - - // resizing command and state vectors - hw_positions_.resize(info_.joints.size(), 0); - // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); // no velocities for our servos - hw_commands_.resize(info_.joints.size(), 0); - - for (const hardware_interface::ComponentInfo & joint : info_.joints) { - if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL( - get_logger(), "Joint '%s' has %zu command interfaces found. 1 expected.", - joint.name.c_str(), joint.command_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - get_logger(), "Joint '%s' have %s command interfaces found. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces.size() != 1) { - RCLCPP_FATAL( - get_logger(), "Joint '%s' has %zu state interface. 1 expected.", joint.name.c_str(), - joint.state_interfaces.size()); - return hardware_interface::CallbackReturn::ERROR; - } - - if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - get_logger(), "Joint '%s' have '%s' as first state interface. '%s' expected.", - joint.name.c_str(), joint.state_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); - return hardware_interface::CallbackReturn::ERROR; - } - } - - auto it_topic = info_.hardware_parameters.find("publisher_topic"); - if (it_topic == info_.hardware_parameters.end() || it_topic->second.empty()) { - RCLCPP_FATAL(get_logger(), "Hardware parameter 'publisher_topic' is missing or empty."); - return hardware_interface::CallbackReturn::ERROR; - } - const std::string publisher_topic = it_topic->second; - - std::string node_name = "lucy_hardware_interface"; - auto it_node = info_.hardware_parameters.find("node_name"); - if (it_node != info_.hardware_parameters.end() && !it_node->second.empty()) { - node_name = it_node->second; - } - node_ = std::make_shared(node_name); - - // RELIABLE to match micro-ROS rclc_subscription_init_default (RELIABLE). A BEST_EFFORT publisher - // does not match a RELIABLE subscription in ROS 2, so the Pico would receive no commands. - rclcpp::QoS qos(rclcpp::KeepLast(10)); - qos.reliable(); - joint_publisher_ = node_->create_publisher(publisher_topic, qos); - - RCLCPP_INFO( - get_logger(), "Publishing joint state on topic '%s' (RELIABLE for micro-ROS default subscriber)", - publisher_topic.c_str()); - - return hardware_interface::CallbackReturn::SUCCESS; -} - - -std::vector LucySystemHardware::export_state_interfaces() -{ - std::vector state_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); - } - - return state_interfaces; -} - -std::vector LucySystemHardware::export_command_interfaces() -{ - std::vector command_interfaces; - for (auto i = 0u; i < info_.joints.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); - } - - return command_interfaces; -} - -/* -This function should be used to initialize/activate the hardware. -In our case, the hardware is already ready to receive informations from the Jetson ? -*/ -hardware_interface::CallbackReturn LucySystemHardware::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(get_logger(), "Successfully activated!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::CallbackReturn LucySystemHardware::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) -{ - RCLCPP_INFO(get_logger(), "Successfully deactivated!"); - - return hardware_interface::CallbackReturn::SUCCESS; -} - -hardware_interface::return_type LucySystemHardware::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // Updating the position of each joint from the command - for (std::size_t i = 0; i < hw_commands_.size(); i++) { - // No encoder for our servos, we assume that the position is always reached - hw_positions_[i] = hw_commands_[i]; - } - - return hardware_interface::return_type::OK; -} - -hardware_interface::return_type ros2_control_demo_example_2::LucySystemHardware::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - // Publish actuator JointState for micro-ROS: Pico uses position[virtual_pin] for 9 outputs - // (see config_*_arm.c). URDF has 10 shoulder/arm joints per arm; omit exactly one per arm so - // the stream has 9 entries in bus order. - // - // Right arm wiring: no servo on URDF right_shoulder_y; shoulder_x is on the bus → omit y only. - // Left arm (stock): no separate bus channel for URDF left_shoulder_x (y,z,elbow,…) → omit x. - sensor_msgs::msg::JointState msg; - msg.header.stamp = node_->get_clock()->now(); - msg.name.clear(); - msg.position.reserve(info_.joints.size()); - for (std::size_t i = 0; i < info_.joints.size(); ++i) { - const std::string & joint_name = info_.joints[i].name; - if (joint_name == "right_shoulder_y_link_joint" || joint_name == "left_shoulder_y_link_joint") { - continue; - } - msg.position.push_back(hw_commands_[i]); - } - - joint_publisher_->publish(msg); - - return hardware_interface::return_type::OK; -} - -} // namespace ros2_control_demo_example_2 - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - ros2_control_demo_example_2::LucySystemHardware, hardware_interface::SystemInterface) diff --git a/lucy_ros2_control/launch/control.launch.py b/lucy_ros2_control/launch/control.launch.py deleted file mode 100644 index e3670c6..0000000 --- a/lucy_ros2_control/launch/control.launch.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python3 -# Real robot only: robot_state_publisher + ros2_control_node + spawners. -# Publishes /actuators/left_arm and /actuators/right_arm for micro-ROS Picos. -# No RViz, no rosbridge. - -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, TimerAction -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - share = get_package_share_directory("thais_urdf") - default_base = os.path.join(share, "inmoov") - default_urdf = os.path.join(default_base, "urdf", "inmoov.urdf.xacro") - urdf_path_arg = DeclareLaunchArgument( - "urdf_path", - default_value=default_urdf, - description="Path to inmoov.urdf.xacro", - ) - base_path_arg = DeclareLaunchArgument( - "base_path", - default_value=default_base, - description="Base path for xacro (mesh_dir)", - ) - urdf_path = LaunchConfiguration("urdf_path") - base_path = LaunchConfiguration("base_path") - - controllers_yaml = PathJoinSubstitution([ - FindPackageShare("lucy_ros2_control"), "config", "lucy_controllers.yaml", - ]) - robot_description = Command(["xacro ", urdf_path, " base_path:=", base_path]) - robot_description_dict = {"robot_description": robot_description} - - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[robot_description_dict], - ) - ros2_control_node = TimerAction( - period=2.0, - actions=[ - Node( - package="controller_manager", - executable="ros2_control_node", - output="screen", - parameters=[controllers_yaml, robot_description_dict], - ) - ], - ) - spawn_joint_state = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - output="screen", - ) - spawn_left = Node( - package="controller_manager", - executable="spawner", - arguments=["left_arm_controller"], - output="screen", - ) - spawn_right = Node( - package="controller_manager", - executable="spawner", - arguments=["right_arm_controller"], - output="screen", - ) - - return LaunchDescription([ - urdf_path_arg, base_path_arg, - robot_state_publisher, ros2_control_node, - spawn_joint_state, spawn_left, spawn_right, - ]) diff --git a/lucy_ros2_control/launch/lucy_launch.xml b/lucy_ros2_control/launch/lucy_launch.xml deleted file mode 100644 index 6e436c1..0000000 --- a/lucy_ros2_control/launch/lucy_launch.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/lucy_ros2_control/lucy_ros2_control.xml b/lucy_ros2_control/lucy_ros2_control.xml index 39e1d32..e322ad6 100644 --- a/lucy_ros2_control/lucy_ros2_control.xml +++ b/lucy_ros2_control/lucy_ros2_control.xml @@ -1,6 +1,6 @@ Our LucySystemHardware. diff --git a/lucy_ros2_control/package.xml b/lucy_ros2_control/package.xml index 90abc2b..3236426 100644 --- a/lucy_ros2_control/package.xml +++ b/lucy_ros2_control/package.xml @@ -3,7 +3,7 @@ lucy_ros2_control 1.0.0 - Package that implements the ROS2 Control configuration for Lucy. Includes launch file, YAML Configuration and Hardware Interface for Lucy + Robot-agnostic ros2_control hardware plugin for Lucy micro-ROS actuator boards. Sentience Robotics Team GPL-3.0 @@ -20,16 +20,11 @@ rviz2 ros_gz_sim rosbridge_server - thais_urdf ament_index_python + ament_cmake_gtest ament_lint_auto ament_lint_common - ament_cmake_pytest - python3-pytest - python3-yaml - ament_index_python - ament_cmake diff --git a/lucy_ros2_control/src/include/joint_config.hpp b/lucy_ros2_control/src/include/joint_config.hpp new file mode 100644 index 0000000..e14e76e --- /dev/null +++ b/lucy_ros2_control/src/include/joint_config.hpp @@ -0,0 +1,121 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// @file joint_config.hpp +/// @brief Pure parsing / validation / mapping helpers for ``LucySystemHardware``. +/// +/// These free functions hold the side-effect-free core of ``on_init``: reading +/// joint parameters, validating ros2_control interfaces, building actuator +/// mappings and computing servo angles. They take plain +/// ``hardware_interface`` structs (no live ROS node), so they are unit-tested +/// in isolation (see ``test/test_joint_config.cpp``). + +#ifndef LUCY_ROS2_CONTROL__JOINT_CONFIG_HPP_ +#define LUCY_ROS2_CONTROL__JOINT_CONFIG_HPP_ + +#include +#include +#include +#include +#include + +#include "hardware_interface/hardware_info.hpp" + +namespace lucy_ros2_control +{ + +/// Convert radians to degrees. +double rad_to_deg(double rad); + +/// Convert degrees to radians. +double deg_to_rad(double deg); + +/// URDF joint-space command envelope (radians); ``±inf`` means "unbounded". +struct JointLimits +{ + double min_rad{-std::numeric_limits::infinity()}; + double max_rad{std::numeric_limits::infinity()}; +}; + +/// Per-actuator calibration + URDF limits parsed from one ros2_control joint. +struct ActuatedJointMapping +{ + std::size_t joint_index{0}; + int virtual_pin{0}; + double offset_deg{0.0}; + double direction{1.0}; + double scale{1.0}; + double servo_min_deg{0.0}; + double servo_max_deg{0.0}; + double servo_default_deg{0.0}; + /// URDF joint-space limits from command_interface min/max (rad). + double min_rad{-std::numeric_limits::infinity()}; + double max_rad{std::numeric_limits::infinity()}; +}; + +/// Parse a numeric string. Throws ``std::runtime_error`` (with ``context``) on +/// any non-numeric input. +double parse_double_string_or_throw(const std::string & raw, const std::string & context); + +/// Fetch and parse a required joint parameter. Throws ``std::runtime_error`` +/// when the key is missing, empty, or non-numeric. +double parse_required_double( + const hardware_interface::ComponentInfo & joint, + const std::string & key); + +/// Parse an optional interface min/max value. Returns ``fallback`` when unset +/// or empty; throws ``std::runtime_error`` when present but non-numeric. +double parse_optional_interface_limit( + const std::optional & value, + const std::string & context, + double fallback); + +/// Validate that a joint exposes exactly one position command interface and one +/// position state interface. Returns an empty string when valid, otherwise a +/// human-readable error message. +std::string validate_joint_interfaces(const hardware_interface::ComponentInfo & joint); + +/// Parse the URDF command_interface ``min``/``max`` for a joint (radians). +/// Missing values default to ``±inf``. Throws ``std::runtime_error`` on +/// non-numeric input. +JointLimits parse_joint_limits(const hardware_interface::ComponentInfo & joint); + +/// Build an actuator mapping for a joint, attaching the URDF limits +/// (``min_rad``/``max_rad``). Returns ``std::nullopt`` when the joint has no +/// ``virtual_pin`` (passive / unmapped). Throws ``std::runtime_error`` on any +/// invalid or non-numeric parameter (bad virtual_pin, zero direction/scale, +/// inverted servo or URDF range). +std::optional build_actuated_joint_mapping( + const hardware_interface::ComponentInfo & joint, + std::size_t joint_index, + double min_rad, + double max_rad); + +/// Default joint position (rad) derived from the actuator's ``servo_default_deg``. +double default_joint_position_rad(const ActuatedJointMapping & m); + +/// Servo angle (rad) published to firmware for a joint-space command (rad): +/// maps joint→servo degrees, clamps to ``[servo_min_deg, servo_max_deg]``, then +/// converts back to radians. +double actuator_command_to_servo_rad(const ActuatedJointMapping & m, double cmd_rad); + +/// Sort ``mappings`` ascending by ``virtual_pin`` and return the first repeated +/// pin, or ``std::nullopt`` when all pins are unique. +std::optional sort_and_find_duplicate_virtual_pin( + std::vector & mappings); + +} // namespace lucy_ros2_control + +#endif // LUCY_ROS2_CONTROL__JOINT_CONFIG_HPP_ diff --git a/lucy_ros2_control/src/include/lucy_system.hpp b/lucy_ros2_control/src/include/lucy_system.hpp new file mode 100644 index 0000000..5cade2e --- /dev/null +++ b/lucy_ros2_control/src/include/lucy_system.hpp @@ -0,0 +1,114 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// @file lucy_system.hpp +/// @brief ros2_control ``SystemInterface`` plugin for Lucy-compatible robots. +/// +/// Maps URDF joint-space commands to servo-space output and clamps them to the +/// ros2_control command_interface ```` envelope (real + +/// mock paths) via ``position_limit_clamp.hpp``. Gazebo uses stock +/// ``gz_ros2_control`` and does not load this plugin. + +#ifndef LUCY_ROS2_CONTROL__LUCY_SYSTEM_HPP_ +#define LUCY_ROS2_CONTROL__LUCY_SYSTEM_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/publisher.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include + +#include "joint_config.hpp" + +namespace lucy_ros2_control +{ +class LucySystemHardware : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(LucySystemHardware) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + rclcpp::Logger get_logger() const {return *logger_;} + // rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + +private: + /// Validate every joint's command/state interfaces (see on_init step 1). + hardware_interface::CallbackReturn validate_joints(); + + /// Read publish/topic/node params and create the actuator publisher + node. + hardware_interface::CallbackReturn configure_publisher(); + + /// Fill joint_min_rad_ / joint_max_rad_ from command_interface min/max. + hardware_interface::CallbackReturn init_joint_limits(); + + /// Build mappings_, seed default positions, sort and reject duplicate pins. + hardware_interface::CallbackReturn init_actuator_mappings(); + + rclcpp::Publisher::SharedPtr joint_publisher_; + rclcpp::Node::SharedPtr node_; + + // Objects for logging + std::shared_ptr logger_; + // rclcpp::Clock::SharedPtr clock_; + + // Store the command for the simulated robot + std::vector hw_commands_; + std::vector hw_positions_; + // std::vector hw_velocities_; // We have no velocity for our servos + + /** Per-joint URDF limits from command_interface min/max (rad); ±inf when unset. */ + std::vector joint_min_rad_; + std::vector joint_max_rad_; + + bool publish_actuators_{true}; + + std::vector mappings_; +}; + +} // namespace lucy_ros2_control + +#endif // LUCY_ROS2_CONTROL__LUCY_SYSTEM_HPP_ diff --git a/lucy_ros2_control/src/include/position_limit_clamp.hpp b/lucy_ros2_control/src/include/position_limit_clamp.hpp new file mode 100644 index 0000000..0131d06 --- /dev/null +++ b/lucy_ros2_control/src/include/position_limit_clamp.hpp @@ -0,0 +1,48 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// @file position_limit_clamp.hpp +/// @brief URDF position-limit clamp for ``LucySystemHardware`` (real + mock). +/// +/// Clamps position commands to the ros2_control command_interface +/// ```` envelope before actuator mapping. Gazebo uses +/// stock ``gz_ros2_control`` and does not apply this helper. + +#ifndef LUCY_ROS2_CONTROL__POSITION_LIMIT_CLAMP_HPP_ +#define LUCY_ROS2_CONTROL__POSITION_LIMIT_CLAMP_HPP_ + +#include +#include + +namespace lucy_ros2_control +{ + +/// Clamp ``cmd`` to ``[min, max]`` ignoring non-finite bounds (treated as no +/// bound on that side). Equivalent to ``std::min(std::max(cmd, min), max)`` +/// when both bounds are finite. +inline double clamp_position_command(double cmd, double min, double max) +{ + if (std::isfinite(min)) { + cmd = std::max(cmd, min); + } + if (std::isfinite(max)) { + cmd = std::min(cmd, max); + } + return cmd; +} + +} // namespace lucy_ros2_control + +#endif // LUCY_ROS2_CONTROL__POSITION_LIMIT_CLAMP_HPP_ diff --git a/lucy_ros2_control/src/joint_config.cpp b/lucy_ros2_control/src/joint_config.cpp new file mode 100644 index 0000000..d5776c5 --- /dev/null +++ b/lucy_ros2_control/src/joint_config.cpp @@ -0,0 +1,204 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include "include/joint_config.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "include/position_limit_clamp.hpp" + +namespace lucy_ros2_control +{ +namespace +{ +constexpr double kPi = 3.14159265358979323846; +} // namespace + +double rad_to_deg(double rad) +{ + return rad * 180.0 / kPi; +} + +double deg_to_rad(double deg) +{ + return deg * kPi / 180.0; +} + +double parse_double_string_or_throw(const std::string & raw, const std::string & context) +{ + try { + return hardware_interface::stod(raw); + } catch (const std::exception & e) { + throw std::runtime_error( + "invalid numeric value '" + raw + "' for " + context + ": " + e.what()); + } +} + +double parse_required_double( + const hardware_interface::ComponentInfo & joint, + const std::string & key) +{ + const auto it = joint.parameters.find(key); + if (it == joint.parameters.end() || it->second.empty()) { + throw std::runtime_error( + "missing required parameter '" + key + "' for joint '" + joint.name + "'"); + } + return parse_double_string_or_throw( + it->second, "joint '" + joint.name + "' parameter '" + key + "'"); +} + +double parse_optional_interface_limit( + const std::optional & value, + const std::string & context, + double fallback) +{ + if (!value.has_value() || value->empty()) { + return fallback; + } + return parse_double_string_or_throw(value.value(), context); +} + +std::string validate_joint_interfaces(const hardware_interface::ComponentInfo & joint) +{ + if (joint.command_interfaces.size() != 1) { + return "Joint '" + joint.name + "' has " + + std::to_string(joint.command_interfaces.size()) + + " command interfaces found. 1 expected."; + } + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + return "Joint '" + joint.name + "' has '" + joint.command_interfaces[0].name + + "' command interface. '" + hardware_interface::HW_IF_POSITION + "' expected."; + } + if (joint.state_interfaces.size() != 1) { + return "Joint '" + joint.name + "' has " + + std::to_string(joint.state_interfaces.size()) + + " state interfaces found. 1 expected."; + } + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + return "Joint '" + joint.name + "' has '" + joint.state_interfaces[0].name + + "' state interface. '" + hardware_interface::HW_IF_POSITION + "' expected."; + } + return std::string(); +} + +JointLimits parse_joint_limits(const hardware_interface::ComponentInfo & joint) +{ + JointLimits limits{}; + if (joint.command_interfaces.empty()) { + return limits; + } + const auto & cmd_if = joint.command_interfaces[0]; + if (cmd_if.name != hardware_interface::HW_IF_POSITION) { + return limits; + } + limits.min_rad = parse_optional_interface_limit( + cmd_if.min, + "joint '" + joint.name + "' command_interface min", + -std::numeric_limits::infinity()); + limits.max_rad = parse_optional_interface_limit( + cmd_if.max, + "joint '" + joint.name + "' command_interface max", + std::numeric_limits::infinity()); + return limits; +} + +std::optional build_actuated_joint_mapping( + const hardware_interface::ComponentInfo & joint, + std::size_t joint_index, + double min_rad, + double max_rad) +{ + const auto it_vpin = joint.parameters.find("virtual_pin"); + if (it_vpin == joint.parameters.end() || it_vpin->second.empty()) { + return std::nullopt; + } + + ActuatedJointMapping m{}; + m.joint_index = joint_index; + try { + m.virtual_pin = std::stoi(it_vpin->second, nullptr, 10); + } catch (const std::exception &) { + throw std::runtime_error( + "invalid virtual_pin '" + it_vpin->second + "' for joint '" + joint.name + "'"); + } + m.offset_deg = parse_required_double(joint, "offset_deg"); + m.direction = parse_required_double(joint, "direction"); + m.scale = parse_required_double(joint, "scale"); + m.servo_min_deg = parse_required_double(joint, "servo_min_deg"); + m.servo_max_deg = parse_required_double(joint, "servo_max_deg"); + m.servo_default_deg = parse_required_double(joint, "servo_default_deg"); + m.min_rad = min_rad; + m.max_rad = max_rad; + + if (m.virtual_pin < 0) { + throw std::runtime_error( + "joint '" + joint.name + "' has invalid virtual_pin " + + std::to_string(m.virtual_pin) + " (must be >= 0)"); + } + if (m.direction == 0.0 || m.scale == 0.0) { + throw std::runtime_error( + "joint '" + joint.name + "' has invalid direction/scale (must be non-zero)"); + } + if (m.servo_min_deg > m.servo_max_deg) { + throw std::runtime_error( + "joint '" + joint.name + "' has servo_min_deg > servo_max_deg"); + } + if (std::isfinite(m.min_rad) && std::isfinite(m.max_rad) && m.min_rad > m.max_rad) { + throw std::runtime_error( + "joint '" + joint.name + "' has command_interface min > max"); + } + + return m; +} + +double default_joint_position_rad(const ActuatedJointMapping & m) +{ + return deg_to_rad((m.servo_default_deg - m.offset_deg) * m.direction * m.scale); +} + +double actuator_command_to_servo_rad(const ActuatedJointMapping & m, double cmd_rad) +{ + const double joint_deg = rad_to_deg(cmd_rad); + const double servo_deg = (joint_deg / (m.direction * m.scale)) + m.offset_deg; + const double clamped_deg = clamp_position_command(servo_deg, m.servo_min_deg, m.servo_max_deg); + return deg_to_rad(clamped_deg); +} + +std::optional sort_and_find_duplicate_virtual_pin( + std::vector & mappings) +{ + std::sort( + mappings.begin(), + mappings.end(), + [](const ActuatedJointMapping & a, const ActuatedJointMapping & b) { + return a.virtual_pin < b.virtual_pin; + }); + for (std::size_t i = 1; i < mappings.size(); ++i) { + if (mappings[i].virtual_pin == mappings[i - 1].virtual_pin) { + return mappings[i].virtual_pin; + } + } + return std::nullopt; +} + +} // namespace lucy_ros2_control diff --git a/lucy_ros2_control/src/lucy_system.cpp b/lucy_ros2_control/src/lucy_system.cpp new file mode 100644 index 0000000..61d35f6 --- /dev/null +++ b/lucy_ros2_control/src/lucy_system.cpp @@ -0,0 +1,295 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include "include/lucy_system.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "include/joint_config.hpp" +#include "include/position_limit_clamp.hpp" +#include "rclcpp/qos.hpp" + +namespace lucy_ros2_control +{ + +hardware_interface::CallbackReturn LucySystemHardware::on_init( + const hardware_interface::HardwareInfo & info) +{ + if ( + hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + logger_ = std::make_shared( + rclcpp::get_logger((info_.name).c_str())); + + // resizing command and state vectors + hw_positions_.resize(info_.joints.size(), 0); + // hw_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); // no velocities for our servos + hw_commands_.resize(info_.joints.size(), 0); + + if (validate_joints() != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + if (configure_publisher() != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + if (init_joint_limits() != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + if (init_actuator_mappings() != hardware_interface::CallbackReturn::SUCCESS) { + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LucySystemHardware::validate_joints() +{ + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + const std::string error = validate_joint_interfaces(joint); + if (!error.empty()) { + RCLCPP_FATAL(get_logger(), "%s", error.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LucySystemHardware::configure_publisher() +{ + auto it_publish = info_.hardware_parameters.find("publish_actuators"); + if (it_publish != info_.hardware_parameters.end()) { + const std::string & v = it_publish->second; + publish_actuators_ = !(v == "false" || v == "0" || v == "False"); + } + + std::string publisher_topic; + auto it_topic = info_.hardware_parameters.find("publisher_topic"); + if (publish_actuators_) { + if (it_topic == info_.hardware_parameters.end() || it_topic->second.empty()) { + RCLCPP_FATAL(get_logger(), "Hardware parameter 'publisher_topic' is missing or empty."); + return hardware_interface::CallbackReturn::ERROR; + } + publisher_topic = it_topic->second; + } + + std::string node_name = "lucy_hardware_interface"; + auto it_node = info_.hardware_parameters.find("node_name"); + if (it_node != info_.hardware_parameters.end() && !it_node->second.empty()) { + node_name = it_node->second; + } + node_ = std::make_shared(node_name); + + // RELIABLE to match micro-ROS rclc_subscription_init_default (RELIABLE). A BEST_EFFORT publisher + // does not match a RELIABLE subscription in ROS 2, so the Pico would receive no commands. + if (publish_actuators_) { + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + joint_publisher_ = node_->create_publisher(publisher_topic, qos); + RCLCPP_INFO( + get_logger(), + "Publishing joint state on topic '%s' (RELIABLE for micro-ROS default subscriber)", + publisher_topic.c_str()); + } else { + RCLCPP_INFO( + get_logger(), + "publish_actuators=false: URDF limits enforced in-process only (no actuator topics)."); + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LucySystemHardware::init_joint_limits() +{ + joint_min_rad_.assign(info_.joints.size(), -std::numeric_limits::infinity()); + joint_max_rad_.assign(info_.joints.size(), std::numeric_limits::infinity()); + + for (std::size_t i = 0; i < info_.joints.size(); ++i) { + try { + const JointLimits limits = parse_joint_limits(info_.joints[i]); + joint_min_rad_[i] = limits.min_rad; + joint_max_rad_[i] = limits.max_rad; + } catch (const std::exception & e) { + RCLCPP_FATAL( + get_logger(), + "Joint '%s' has invalid command_interface limits: %s", + info_.joints[i].name.c_str(), + e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + } + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LucySystemHardware::init_actuator_mappings() +{ + mappings_.clear(); + mappings_.reserve(info_.joints.size()); + + for (std::size_t i = 0; i < info_.joints.size(); ++i) { + const auto & joint = info_.joints[i]; + + std::optional mapping; + try { + mapping = build_actuated_joint_mapping(joint, i, joint_min_rad_[i], joint_max_rad_[i]); + } catch (const std::exception & e) { + RCLCPP_FATAL( + get_logger(), "Joint '%s' has invalid parameters: %s", joint.name.c_str(), + e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + if (!mapping.has_value()) { + RCLCPP_WARN( + get_logger(), + "Joint '%s' has no virtual_pin; treated as passive/unmapped for actuator output.", + joint.name.c_str()); + continue; + } + + const ActuatedJointMapping & m = mapping.value(); + if (!std::isfinite(m.min_rad) || !std::isfinite(m.max_rad)) { + RCLCPP_WARN( + get_logger(), + "Joint '%s' has no finite URDF position limits on command interface; " + "only servo_min/max_deg will bound output.", + joint.name.c_str()); + } + + mappings_.push_back(m); + hw_commands_[i] = default_joint_position_rad(m); + hw_positions_[i] = hw_commands_[i]; + } + + const std::optional duplicate = sort_and_find_duplicate_virtual_pin(mappings_); + if (duplicate.has_value()) { + RCLCPP_FATAL( + get_logger(), "Duplicate virtual_pin %d in hardware '%s'.", duplicate.value(), + info_.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + + +std::vector LucySystemHardware::export_state_interfaces() +{ + std::vector state_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_positions_[i])); + } + + return state_interfaces; +} + +std::vector LucySystemHardware::export_command_interfaces() +{ + std::vector command_interfaces; + for (auto i = 0u; i < info_.joints.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + } + + return command_interfaces; +} + +/* +This function should be used to initialize/activate the hardware. +In our case, the hardware is already ready to receive informations +*/ +hardware_interface::CallbackReturn LucySystemHardware::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(get_logger(), "Successfully activated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn LucySystemHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(get_logger(), "Successfully deactivated!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::return_type LucySystemHardware::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + // Updating the position of each joint from the command + for (std::size_t i = 0; i < hw_commands_.size(); i++) { + // No encoder for our servos, we assume that the position is always reached + hw_positions_[i] = hw_commands_[i]; + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type lucy_ros2_control::LucySystemHardware::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (std::size_t i = 0; i < hw_commands_.size(); ++i) { + const double cmd_rad = lucy_ros2_control::clamp_position_command( + hw_commands_[i], joint_min_rad_[i], joint_max_rad_[i]); + hw_commands_[i] = cmd_rad; + hw_positions_[i] = cmd_rad; + } + + if (!publish_actuators_ || !joint_publisher_) { + return hardware_interface::return_type::OK; + } + + // Firmware reads inputs->position.data[joint->config.virtual_pin] per configured joint. + sensor_msgs::msg::JointState msg; + msg.header.stamp = node_->get_clock()->now(); + msg.name.clear(); + + if (mappings_.empty()) { + joint_publisher_->publish(msg); + return hardware_interface::return_type::OK; + } + + int max_vp = mappings_.back().virtual_pin; + msg.position.assign(static_cast(max_vp) + 1U, 0.0); + + for (const auto & m : mappings_) { + msg.position[static_cast(m.virtual_pin)] = + actuator_command_to_servo_rad(m, hw_commands_[m.joint_index]); + } + + joint_publisher_->publish(msg); + + return hardware_interface::return_type::OK; +} + +} // namespace lucy_ros2_control + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + lucy_ros2_control::LucySystemHardware, hardware_interface::SystemInterface) diff --git a/lucy_ros2_control/test/test_controllers_yaml.py b/lucy_ros2_control/test/test_controllers_yaml.py deleted file mode 100644 index 8696b0e..0000000 --- a/lucy_ros2_control/test/test_controllers_yaml.py +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright 2025 Sentience Robotics Team -# SPDX-License-Identifier: GPL-3.0 -"""Validate lucy_controllers.yaml loads and contains expected ros2_control configuration.""" -from pathlib import Path - -import yaml - -try: - from ament_index_python.packages import get_package_share_directory -except ImportError: - get_package_share_directory = None - - -def _controllers_path() -> Path: - if get_package_share_directory is not None: - try: - share = Path(get_package_share_directory('lucy_ros2_control')) - p = share / 'config' / 'lucy_controllers.yaml' - if p.is_file(): - return p - except Exception: - pass - # Source checkout: lucy_ros2_control/config/... - return Path(__file__).resolve().parents[1] / 'config' / 'lucy_controllers.yaml' - - -def test_controllers_yaml_exists(): - path = _controllers_path() - assert path.is_file(), f'missing {path}' - - -def test_controllers_yaml_structure(): - path = _controllers_path() - with path.open('r', encoding='utf-8') as f: - data = yaml.safe_load(f) - assert isinstance(data, dict), 'root must be a mapping' - assert 'controller_manager' in data - cm = data['controller_manager']['ros__parameters'] - assert 'update_rate' in cm - for name in ('joint_state_broadcaster', 'left_arm_controller', 'right_arm_controller'): - assert name in cm, f'controller_manager must declare {name}' - assert 'joint_state_broadcaster' in data - extra = data['joint_state_broadcaster']['ros__parameters'].get('extra_joints', []) - assert isinstance(extra, list) - for name in ('left_arm_controller', 'right_arm_controller'): - assert name in data - joints = data[name]['ros__parameters']['joints'] - assert isinstance(joints, list) and len(joints) > 0 diff --git a/lucy_ros2_control/test/test_joint_config.cpp b/lucy_ros2_control/test/test_joint_config.cpp new file mode 100644 index 0000000..886ed5d --- /dev/null +++ b/lucy_ros2_control/test/test_joint_config.cpp @@ -0,0 +1,390 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// @file test_joint_config.cpp +/// @brief Unit tests for the pure on_init helpers in joint_config.hpp. +/// +/// Exercises parsing, interface validation, actuator-mapping construction and +/// the joint<->servo angle math without a live ROS node, using plain +/// hardware_interface structs. + +#include +#include +#include +#include +#include + +#include + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "joint_config.hpp" + +namespace +{ +using hardware_interface::ComponentInfo; +using hardware_interface::InterfaceInfo; +using lucy_ros2_control::ActuatedJointMapping; + +constexpr double kInf = std::numeric_limits::infinity(); +constexpr double kPi = 3.14159265358979323846; + +InterfaceInfo position_interface() +{ + InterfaceInfo info; + info.name = hardware_interface::HW_IF_POSITION; + return info; +} + +/// A fully valid position-controlled actuator joint with sane calibration. +ComponentInfo make_valid_joint(const std::string & name = "joint_a") +{ + ComponentInfo joint; + joint.name = name; + joint.command_interfaces = {position_interface()}; + joint.state_interfaces = {position_interface()}; + joint.parameters = { + {"virtual_pin", "3"}, + {"offset_deg", "90"}, + {"direction", "1"}, + {"scale", "1"}, + {"servo_min_deg", "0"}, + {"servo_max_deg", "180"}, + {"servo_default_deg", "90"}, + }; + return joint; +} + +ActuatedJointMapping make_mapping(int virtual_pin) +{ + ActuatedJointMapping m; + m.virtual_pin = virtual_pin; + return m; +} +} // namespace + +// --------------------------------------------------------------------------- +// Angle conversions +// --------------------------------------------------------------------------- + +TEST(JointConfigConversions, DegRadRoundTrip) +{ + EXPECT_NEAR(lucy_ros2_control::deg_to_rad(180.0), kPi, 1e-12); + EXPECT_NEAR(lucy_ros2_control::rad_to_deg(kPi), 180.0, 1e-12); + EXPECT_NEAR(lucy_ros2_control::rad_to_deg(lucy_ros2_control::deg_to_rad(42.0)), 42.0, 1e-12); +} + +// --------------------------------------------------------------------------- +// Numeric parsing +// --------------------------------------------------------------------------- + +TEST(JointConfigParse, ParsesValidDouble) +{ + EXPECT_DOUBLE_EQ(lucy_ros2_control::parse_double_string_or_throw("1.5", "ctx"), 1.5); + EXPECT_DOUBLE_EQ(lucy_ros2_control::parse_double_string_or_throw("-3", "ctx"), -3.0); +} + +TEST(JointConfigParse, ThrowsOnNonNumeric) +{ + EXPECT_THROW( + lucy_ros2_control::parse_double_string_or_throw("sdhjsdf", "ctx"), + std::runtime_error); +} + +TEST(JointConfigParse, RequiredDoublePresent) +{ + const ComponentInfo joint = make_valid_joint(); + EXPECT_DOUBLE_EQ(lucy_ros2_control::parse_required_double(joint, "offset_deg"), 90.0); +} + +TEST(JointConfigParse, RequiredDoubleMissingThrows) +{ + const ComponentInfo joint = make_valid_joint(); + EXPECT_THROW(lucy_ros2_control::parse_required_double(joint, "no_such_key"), std::runtime_error); +} + +TEST(JointConfigParse, RequiredDoubleEmptyThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters["offset_deg"] = ""; + EXPECT_THROW(lucy_ros2_control::parse_required_double(joint, "offset_deg"), std::runtime_error); +} + +TEST(JointConfigParse, RequiredDoubleNonNumericThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters["scale"] = "abc"; + EXPECT_THROW(lucy_ros2_control::parse_required_double(joint, "scale"), std::runtime_error); +} + +TEST(JointConfigParse, OptionalInterfaceLimitFallsBackWhenEmpty) +{ + EXPECT_DOUBLE_EQ( + lucy_ros2_control::parse_optional_interface_limit(std::nullopt, "ctx", -kInf), -kInf); + EXPECT_DOUBLE_EQ( + lucy_ros2_control::parse_optional_interface_limit(std::string(""), "ctx", 7.0), 7.0); +} + +TEST(JointConfigParse, OptionalInterfaceLimitParsesAndThrows) +{ + EXPECT_DOUBLE_EQ( + lucy_ros2_control::parse_optional_interface_limit(std::string("1.25"), "ctx", 0.0), 1.25); + EXPECT_THROW( + lucy_ros2_control::parse_optional_interface_limit(std::string("nope"), "ctx", 0.0), + std::runtime_error); +} + +// --------------------------------------------------------------------------- +// Interface validation +// --------------------------------------------------------------------------- + +TEST(JointConfigValidate, ValidJointReturnsEmpty) +{ + EXPECT_TRUE(lucy_ros2_control::validate_joint_interfaces(make_valid_joint()).empty()); +} + +TEST(JointConfigValidate, WrongCommandInterfaceCountFails) +{ + ComponentInfo joint = make_valid_joint(); + joint.command_interfaces.clear(); + EXPECT_FALSE(lucy_ros2_control::validate_joint_interfaces(joint).empty()); +} + +TEST(JointConfigValidate, WrongCommandInterfaceNameFails) +{ + ComponentInfo joint = make_valid_joint(); + joint.command_interfaces[0].name = hardware_interface::HW_IF_VELOCITY; + EXPECT_FALSE(lucy_ros2_control::validate_joint_interfaces(joint).empty()); +} + +TEST(JointConfigValidate, WrongStateInterfaceCountFails) +{ + ComponentInfo joint = make_valid_joint(); + joint.state_interfaces.push_back(position_interface()); + EXPECT_FALSE(lucy_ros2_control::validate_joint_interfaces(joint).empty()); +} + +TEST(JointConfigValidate, WrongStateInterfaceNameFails) +{ + ComponentInfo joint = make_valid_joint(); + joint.state_interfaces[0].name = hardware_interface::HW_IF_EFFORT; + EXPECT_FALSE(lucy_ros2_control::validate_joint_interfaces(joint).empty()); +} + +// --------------------------------------------------------------------------- +// URDF command_interface limits +// --------------------------------------------------------------------------- + +TEST(JointConfigLimits, UnsetLimitsDefaultToInfinity) +{ + const lucy_ros2_control::JointLimits limits = + lucy_ros2_control::parse_joint_limits(make_valid_joint()); + EXPECT_FALSE(std::isfinite(limits.min_rad)); + EXPECT_FALSE(std::isfinite(limits.max_rad)); + EXPECT_LT(limits.min_rad, 0.0); + EXPECT_GT(limits.max_rad, 0.0); +} + +TEST(JointConfigLimits, ParsesProvidedLimits) +{ + ComponentInfo joint = make_valid_joint(); + joint.command_interfaces[0].min = "-1.5708"; + joint.command_interfaces[0].max = "1.5708"; + const lucy_ros2_control::JointLimits limits = lucy_ros2_control::parse_joint_limits(joint); + EXPECT_DOUBLE_EQ(limits.min_rad, -1.5708); + EXPECT_DOUBLE_EQ(limits.max_rad, 1.5708); +} + +TEST(JointConfigLimits, NonNumericLimitThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.command_interfaces[0].min = "not_a_number"; + EXPECT_THROW(lucy_ros2_control::parse_joint_limits(joint), std::runtime_error); +} + +// --------------------------------------------------------------------------- +// Actuator mapping construction +// --------------------------------------------------------------------------- + +TEST(JointConfigMapping, BuildsValidMapping) +{ + const ComponentInfo joint = make_valid_joint(); + const auto m = lucy_ros2_control::build_actuated_joint_mapping(joint, 7, -1.0, 2.0); + ASSERT_TRUE(m.has_value()); + EXPECT_EQ(m->joint_index, 7u); + EXPECT_EQ(m->virtual_pin, 3); + EXPECT_DOUBLE_EQ(m->offset_deg, 90.0); + EXPECT_DOUBLE_EQ(m->servo_default_deg, 90.0); + EXPECT_DOUBLE_EQ(m->min_rad, -1.0); + EXPECT_DOUBLE_EQ(m->max_rad, 2.0); +} + +TEST(JointConfigMapping, NoVirtualPinReturnsNullopt) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters.erase("virtual_pin"); + EXPECT_FALSE( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf).has_value()); +} + +TEST(JointConfigMapping, NonFiniteLimitsStillBuild) +{ + const ComponentInfo joint = make_valid_joint(); + const auto m = lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf); + ASSERT_TRUE(m.has_value()); + EXPECT_FALSE(std::isfinite(m->min_rad)); + EXPECT_FALSE(std::isfinite(m->max_rad)); +} + +TEST(JointConfigMapping, NegativeVirtualPinThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters["virtual_pin"] = "-1"; + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf), std::runtime_error); +} + +TEST(JointConfigMapping, NonNumericVirtualPinThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters["virtual_pin"] = "pin5"; + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf), std::runtime_error); +} + +TEST(JointConfigMapping, ZeroDirectionOrScaleThrows) +{ + ComponentInfo zero_dir = make_valid_joint(); + zero_dir.parameters["direction"] = "0"; + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(zero_dir, 0, -kInf, kInf), std::runtime_error); + + ComponentInfo zero_scale = make_valid_joint(); + zero_scale.parameters["scale"] = "0"; + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(zero_scale, 0, -kInf, kInf), + std::runtime_error); +} + +TEST(JointConfigMapping, InvertedServoRangeThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters["servo_min_deg"] = "120"; + joint.parameters["servo_max_deg"] = "30"; + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf), std::runtime_error); +} + +TEST(JointConfigMapping, InvertedFiniteUrdfRangeThrows) +{ + const ComponentInfo joint = make_valid_joint(); + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, 2.0, -2.0), std::runtime_error); +} + +TEST(JointConfigMapping, MissingCalibrationParamThrows) +{ + ComponentInfo joint = make_valid_joint(); + joint.parameters.erase("offset_deg"); + EXPECT_THROW( + lucy_ros2_control::build_actuated_joint_mapping(joint, 0, -kInf, kInf), std::runtime_error); +} + +// --------------------------------------------------------------------------- +// Joint <-> servo math +// --------------------------------------------------------------------------- + +TEST(JointConfigMath, DefaultJointPositionRad) +{ + ActuatedJointMapping m; + m.offset_deg = 90.0; + m.direction = 1.0; + m.scale = 1.0; + m.servo_default_deg = 90.0; + // (90 - 90) * 1 * 1 = 0 deg -> 0 rad + EXPECT_NEAR(lucy_ros2_control::default_joint_position_rad(m), 0.0, 1e-12); + + m.servo_default_deg = 135.0; + // (135 - 90) = 45 deg -> pi/4 + EXPECT_NEAR(lucy_ros2_control::default_joint_position_rad(m), kPi / 4.0, 1e-12); +} + +TEST(JointConfigMath, ActuatorCommandToServoRadWithinRange) +{ + ActuatedJointMapping m; + m.offset_deg = 90.0; + m.direction = 1.0; + m.scale = 1.0; + m.servo_min_deg = 0.0; + m.servo_max_deg = 180.0; + // cmd 0 rad -> joint 0 deg -> servo 90 deg -> pi/2 rad + EXPECT_NEAR(lucy_ros2_control::actuator_command_to_servo_rad(m, 0.0), kPi / 2.0, 1e-12); +} + +TEST(JointConfigMath, ActuatorCommandToServoRadClampsHigh) +{ + ActuatedJointMapping m; + m.offset_deg = 90.0; + m.direction = 1.0; + m.scale = 1.0; + m.servo_min_deg = 0.0; + m.servo_max_deg = 180.0; + // cmd pi rad -> joint 180 deg -> servo 270 deg -> clamps to 180 deg -> pi rad + EXPECT_NEAR(lucy_ros2_control::actuator_command_to_servo_rad(m, kPi), kPi, 1e-12); +} + +TEST(JointConfigMath, ActuatorCommandHonoursDirection) +{ + ActuatedJointMapping m; + m.offset_deg = 90.0; + m.direction = -1.0; + m.scale = 1.0; + m.servo_min_deg = 0.0; + m.servo_max_deg = 180.0; + // cmd pi/2 rad -> joint 90 deg -> servo (90 / -1) + 90 = 0 deg -> 0 rad + EXPECT_NEAR(lucy_ros2_control::actuator_command_to_servo_rad(m, kPi / 2.0), 0.0, 1e-12); +} + +// --------------------------------------------------------------------------- +// Duplicate virtual_pin detection / sort +// --------------------------------------------------------------------------- + +TEST(JointConfigDuplicates, UniquePinsSortAscendingNoDuplicate) +{ + std::vector mappings = { + make_mapping(5), make_mapping(1), make_mapping(3)}; + const auto dup = lucy_ros2_control::sort_and_find_duplicate_virtual_pin(mappings); + EXPECT_FALSE(dup.has_value()); + ASSERT_EQ(mappings.size(), 3u); + EXPECT_EQ(mappings[0].virtual_pin, 1); + EXPECT_EQ(mappings[1].virtual_pin, 3); + EXPECT_EQ(mappings[2].virtual_pin, 5); +} + +TEST(JointConfigDuplicates, DetectsDuplicatePin) +{ + std::vector mappings = { + make_mapping(2), make_mapping(4), make_mapping(2)}; + const auto dup = lucy_ros2_control::sort_and_find_duplicate_virtual_pin(mappings); + ASSERT_TRUE(dup.has_value()); + EXPECT_EQ(dup.value(), 2); +} + +TEST(JointConfigDuplicates, EmptyListHasNoDuplicate) +{ + std::vector mappings; + EXPECT_FALSE(lucy_ros2_control::sort_and_find_duplicate_virtual_pin(mappings).has_value()); +} diff --git a/lucy_ros2_control/test/test_position_limit_clamp.cpp b/lucy_ros2_control/test/test_position_limit_clamp.cpp new file mode 100644 index 0000000..10fa3b9 --- /dev/null +++ b/lucy_ros2_control/test/test_position_limit_clamp.cpp @@ -0,0 +1,100 @@ +// Copyright 2025 Sentience Robotics Team +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +/// @file test_position_limit_clamp.cpp +/// @brief Unit tests for the URDF position-limit clamp in LucySystemHardware. +/// +/// Real and mock hardware clamp incoming position commands to the ros2_control +/// ``[min, max]`` envelope. Gazebo uses stock ``gz_ros2_control`` without this +/// clamp; this helper documents the algorithm used on the Lucy plugin path. + +#include +#include + +#include + +#include "position_limit_clamp.hpp" + +namespace +{ +constexpr double kInf = std::numeric_limits::infinity(); +constexpr double kNan = std::numeric_limits::quiet_NaN(); + +double clamp(double cmd, double min, double max) +{ + return lucy_ros2_control::clamp_position_command(cmd, min, max); +} +} // namespace + +TEST(PositionLimitClamp, BelowMinClampsToMin) +{ + EXPECT_DOUBLE_EQ(clamp(-1.0, 0.0, 1.5708), 0.0); +} + +TEST(PositionLimitClamp, AboveMaxClampsToMax) +{ + EXPECT_DOUBLE_EQ(clamp(4.71239, 0.0, 1.5708), 1.5708); +} + +TEST(PositionLimitClamp, WithinRangeIsUnchanged) +{ + EXPECT_DOUBLE_EQ(clamp(0.5, 0.0, 1.5708), 0.5); +} + +TEST(PositionLimitClamp, OnExactBoundsIsUnchanged) +{ + EXPECT_DOUBLE_EQ(clamp(0.0, 0.0, 1.5708), 0.0); + EXPECT_DOUBLE_EQ(clamp(1.5708, 0.0, 1.5708), 1.5708); +} + +TEST(PositionLimitClamp, DegeneratePointRangePinsToValue) +{ + EXPECT_DOUBLE_EQ(clamp(-2.0, 0.5, 0.5), 0.5); + EXPECT_DOUBLE_EQ(clamp(2.0, 0.5, 0.5), 0.5); +} + +TEST(PositionLimitClamp, NonFiniteMinDisablesLowerBound) +{ + EXPECT_DOUBLE_EQ(clamp(-1.0e9, -kInf, 1.5708), -1.0e9); + EXPECT_DOUBLE_EQ(clamp(2.0, -kInf, 1.5708), 1.5708); + EXPECT_DOUBLE_EQ(clamp(0.5, kNan, 1.5708), 0.5); +} + +TEST(PositionLimitClamp, NonFiniteMaxDisablesUpperBound) +{ + EXPECT_DOUBLE_EQ(clamp(1.0e9, 0.0, kInf), 1.0e9); + EXPECT_DOUBLE_EQ(clamp(-2.0, 0.0, kInf), 0.0); + EXPECT_DOUBLE_EQ(clamp(0.5, 0.0, kNan), 0.5); +} + +TEST(PositionLimitClamp, BothBoundsNonFiniteIsNoOp) +{ + EXPECT_DOUBLE_EQ(clamp(1.0e9, -kInf, kInf), 1.0e9); + EXPECT_DOUBLE_EQ(clamp(-1.0e9, -kInf, kInf), -1.0e9); + EXPECT_DOUBLE_EQ(clamp(0.0, kNan, kNan), 0.0); +} + +/// Representative grid pins the clamp algorithm used by LucySystemHardware. +TEST(PositionLimitClamp, ClampAlgorithmOnRepresentativeGrid) +{ + // (lower, upper) drawn from a real ``left_shoulder_y_link_joint`` envelope. + const double lo = 0.0; + const double hi = 1.5708; + const double grid[] = {-3.14159, -0.0001, 0.0, 0.5, 1.0, 1.5707, 1.5708, 1.5709, 4.71239}; + for (double cmd : grid) { + const double expected = std::min(std::max(cmd, lo), hi); + EXPECT_DOUBLE_EQ(clamp(cmd, lo, hi), expected) << "cmd=" << cmd; + } +}