Implementation of ROS2 Control Package

.github/workflows/ci.yml

@@ -1,26 +1,31 @@
 name: CI
 
 on:
-  push:
-    branches: [ main, dev ]
   pull_request:
-    branches: [ main, dev ]
+  push:
+    branches:
+      - main
+      - master
+      - dev
 
 jobs:
-  lint:
-    name: Lint
+  build-and-test:
+    name: Build and test
     runs-on: ubuntu-latest
     container:
       image: osrf/ros:humble-desktop
     steps:
       - uses: actions/checkout@v4
-      
-      - name: Install dependencies
+
+      - name: Install system and ROS dependencies
         run: |
-          apt-get update && apt-get install -y \
+          apt-get update && apt-get install -y --no-install-recommends \
             python3-pip \
+            python3-rosdep \
             python3-pytest \
             python3-pytest-mock \
+            python3-pytest-cov \
+            python3-yaml \
             python3-opencv \
             gstreamer1.0-tools \
             gstreamer1.0-plugins-base \
@@ -30,126 +35,99 @@ jobs:
             v4l-utils \
             build-essential \
             cmake \
-            git \
-            && rm -rf /var/lib/apt/lists/*
-
-      - name: Setup workspace structure
+            git
+          rosdep init || true
+          rosdep update
+
+      - name: Workspace layout
         run: |
           mkdir -p ws/src
-          cp -r camera_ros lucy_bringup ws/src/
-
-      - name: Build workspace
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
-        shell: bash
-        run: |
-          source /opt/ros/humble/setup.bash
-          colcon build --packages-select camera_ros lucy_bringup --cmake-args -DBUILD_TESTING=ON
-
-      - name: Run linters
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
-        shell: bash
-        run: |
-          source /opt/ros/humble/setup.bash
-          source install/setup.bash
-          colcon test --packages-select camera_ros lucy_bringup --event-handlers console_direct+ || true
-
-      - name: Check lint results
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
+          cp -r camera_ros lucy_bringup lucy_ros2_control 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.
+      - name: rosdep install
+        working-directory: ws
         shell: bash
         run: |
+          apt-get update
           source /opt/ros/humble/setup.bash
-          source install/setup.bash
-          colcon test-result --verbose || true
+          rosdep install --from-paths src --ignore-src -r -y \
+            --skip-keys="audio_common micro_ros_agent realsense2_camera thais_urdf"
 
-  test:
-    name: Unit Tests
-    runs-on: ubuntu-latest
-    container:
-      image: osrf/ros:humble-desktop
-    steps:
-      - uses: actions/checkout@v4
-
-      - name: Install dependencies
-        run: |
-          apt-get update && apt-get install -y \
-            python3-pip \
-            python3-pytest \
-            python3-pytest-mock \
-            python3-opencv \
-            gstreamer1.0-tools \
-            gstreamer1.0-plugins-base \
-            gstreamer1.0-plugins-good \
-            gstreamer1.0-plugins-bad \
-            gstreamer1.0-plugins-ugly \
-            v4l-utils \
-            build-essential \
-            cmake \
-            git \
-            && rm -rf /var/lib/apt/lists/*
-
-      - name: Install Python test dependencies
-        run: |
-          pip3 install --no-cache-dir pytest pytest-mock pytest-cov
-
-      - name: Setup workspace structure
-        run: |
-          mkdir -p ws/src
-          cp -r camera_ros lucy_bringup ws/src/
-
-      - name: Build workspace with tests
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
+      - name: colcon build
+        working-directory: ws
         shell: bash
         run: |
           source /opt/ros/humble/setup.bash
-          colcon build --packages-select camera_ros lucy_bringup --cmake-args -DBUILD_TESTING=ON
-
-      - name: Run unit tests
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
+          colcon build --symlink-install \
+            --packages-select camera_ros lucy_bringup lucy_ros2_control \
+            --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=ON
+
+      - name: colcon test
+        working-directory: ws
         shell: bash
         run: |
           source /opt/ros/humble/setup.bash
           source install/setup.bash
-          colcon test --packages-select camera_ros lucy_bringup --event-handlers console_direct+
-
-      - name: Check test results
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
+          colcon test \
+            --packages-select camera_ros lucy_bringup lucy_ros2_control \
+            --event-handlers console_direct+ \
+            --pytest-args "--tb=short -vv"
+
+      - name: colcon test-result
+        working-directory: ws
         shell: bash
         run: |
           source /opt/ros/humble/setup.bash
           source install/setup.bash
           colcon test-result --verbose
-          # Fail if any tests failed
-          if colcon test-result --verbose | grep -q "failed"; then
-            echo "Some tests failed!"
-            exit 1
-          fi
-
-      - name: Generate coverage report
-        working-directory: /__w/lucy_ros_packages/lucy_ros_packages/ws
+
+      - name: Code coverage (pytest-cov, all Python tests)
+        working-directory: ws
         shell: bash
+        if: success()
         run: |
+          set -e
           source /opt/ros/humble/setup.bash
           source install/setup.bash
-          cd build/camera_ros
-          python3 -m pytest ../../src/camera_ros/test/ \
-            --cov=../../src/camera_ros/scripts \
-            --cov-report=xml:coverage.xml \
-            --cov-report=html:htmlcov \
+          mkdir -p build/coverage_reports
+          # camera_ros — production code under scripts/
+          python3 -m pytest src/camera_ros/test/ \
+            --cov=src/camera_ros/scripts \
+            --cov-report=xml:build/coverage_reports/camera_ros.xml \
+            --cov-report=html:build/coverage_reports/html_camera_ros \
+            --cov-report=term-missing \
+            --cov-branch
+          # lucy_bringup — launch/*.py (py_compile tests do not execute module bodies; expect low %)
+          python3 -m pytest src/lucy_bringup/test/ \
+            --cov=src/lucy_bringup/launch \
+            --cov-report=xml:build/coverage_reports/lucy_bringup.xml \
+            --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 \
             --cov-report=term-missing \
-            --cov-branch || true
-      
+            --cov-branch
+
       - name: Upload coverage to Codecov
-        uses: codecov/codecov-action@v3
+        uses: codecov/codecov-action@v4
+        if: success()
         with:
-          file: ${{ github.workspace }}/ws/build/camera_ros/coverage.xml
-          flags: unittests
-          name: codecov-umbrella
+          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
+          flags: lucy_ros_packages
+          name: lucy_ros_packages
           fail_ci_if_error: false
-      
-      - name: Upload coverage HTML report
+
+      - name: Upload coverage artifacts (XML + HTML)
         uses: actions/upload-artifact@v4
         if: always()
         with:
-          name: coverage-report
-          path: ${{ github.workspace }}/ws/build/camera_ros/htmlcov/
-          retention-days: 30
+          name: coverage-lucy_ros_packages
+          path: ${{ github.workspace }}/ws/build/coverage_reports/
+          retention-days: 14

README.md

@@ -0,0 +1,129 @@
+# lucy_ros_packages
+
+ROS 2 **Humble** repository for **Lucy** (Sentience Robotics): runtime bringup, `ros2_control` hardware integration, and camera tooling. This is a **multi-package** repo; each subdirectory under `src/` in your colcon workspace is one `ament` package.
+
+## What lives here
+
+| 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_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). |
+| [**camera_ros**](camera_ros/) | GStreamer-based **MJPEG** → `sensor_msgs/CompressedImage`; client-aware activation. |
+
+Package names match directories (`<name>` 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.
+- **Web control panel** and teleop semantics are **not** in this repo; they consume the same topics/controllers documented in lucy_ws docs.
+
+## Requirements
+
+- **OS**: Ubuntu 22.04
+- **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`.
+
+## 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/`:
+
+```text
+lucy_ws/
+└── src/
+    ├── lucy_ros_packages/    # this repo: lucy_bringup, lucy_ros2_control, camera_ros
+    └── thais_urdf/           # robot description + sim launches (separate repo)
+```
+
+Example build:
+
+```bash
+source /opt/ros/humble/setup.bash
+cd lucy_ws
+colcon build --symlink-install \
+  --packages-select lucy_bringup lucy_ros2_control camera_ros
+source install/setup.bash
+```
+
+To include simulation/description from the other repo:
+
+```bash
+colcon build --symlink-install \
+  --packages-select lucy_bringup lucy_ros2_control camera_ros thais_urdf
+```
+
+## Quick start
+
+| Goal | Entry point |
+|------|-------------|
+| Full Jetson stack (agents, rosbridge, cameras, `ros2_control`) | `ros2 launch lucy_bringup lucy.launch.py` |
+| Control stack only (defaults assume `thais_urdf` layout in workspace) | `ros2 launch lucy_ros2_control control.launch.py` |
+| USB MJPEG camera node | `ros2 launch camera_ros camera.launch.py` |
+
+Tmux helpers ship with `lucy_bringup` (`system_scripts/`); see [lucy_bringup/README.md](lucy_bringup/README.md).
+
+## Tests and coverage (local)
+
+**Dependencies:** `python3-pytest-cov` (e.g. `sudo apt install python3-pytest-cov` on Ubuntu).
+
+From your **workspace root** (e.g. `lucy_ws`), with packages under `src/lucy_ros_packages/`:
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install \
+  --packages-select camera_ros lucy_bringup lucy_ros2_control \
+  --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-result --verbose
+```
+
+**Coverage** (same `source install/setup.bash` as above; open `build/coverage_html/.../index.html` after `--cov-report=html`):
+
+```bash
+mkdir -p build/coverage_reports build/coverage_html
+python3 -m pytest src/lucy_ros_packages/camera_ros/test/ \
+  --cov=src/lucy_ros_packages/camera_ros/scripts \
+  --cov-report=term-missing \
+  --cov-report=xml:build/coverage_reports/camera_ros.xml \
+  --cov-report=html:build/coverage_html/camera_ros
+
+python3 -m pytest src/lucy_ros_packages/lucy_bringup/test/ \
+  --cov=src/lucy_ros_packages/lucy_bringup/launch \
+  --cov-report=term-missing \
+  --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.
+
+## 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.
+
+## Documentation map
+
+| Doc | Audience |
+|-----|----------|
+| This file | Anyone cloning **this** repository |
+| [**doc/DEVELOPER.md**](doc/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) |
+| [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).
+
+## License
+
+Packages in this repository are licensed under **GPL-3.0** unless a subdirectory states otherwise. See each package and the repository **`LICENSE`** file if present.
+
+## Maintainer
+
+Sentience Robotics Team — `contact@sentience-robotics.fr` (see `package.xml` files).

camera_ros/CMakeLists.txt

@@ -22,6 +22,17 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   ADD_LINTER_TESTS
 )
 
+# Bind-mounted / persisted build dirs can leave .../ament_cmake_python/<pkg>/<pkg> 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})
+  add_custom_target(_camera_ros_clear_stale_ament_python_symlink
+    COMMAND bash -c "rm -rf -- '${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_python/${PROJECT_NAME}/${PROJECT_NAME}'"
+    VERBATIM
+    COMMENT "Remove stale path before ament_cmake_python symlink (Docker bind mounts)"
+  )
+  add_dependencies(ament_cmake_python_symlink_${PROJECT_NAME} _camera_ros_clear_stale_ament_python_symlink)
+endif()
+
 # Install Python scripts
 install(PROGRAMS
   scripts/camera_publisher.py