Add C++17 unit tests and fix CI to use ros:noetic-robot Docker image

C++ unit tests (gps_agent_pkg/test/):
- test_options_variant.cpp: 28 tests for std::variant OptionsVariant and
  OptionsMap — exercises holds_alternative, std::get, bad_variant_access,
  rebinding, and simulated controller config patterns
- test_util.cpp: 13 tests for util::split() and to_string<T>() (pure stdlib)
- test_obs_normalization.cpp: 13 tests for the PyTorchController obs
  normalization formula extracted verbatim from pytorchcontroller.cpp;
  covers identity, NaN/Inf propagation, and linearity properties
- test/CMakeLists.txt: standalone build (no ROS, no LibTorch) used by the
  TSan CI job to avoid LibTorch+TSan incompatibility

gps_agent_pkg/CMakeLists.txt:
- Add find_package(Protobuf REQUIRED) + add_custom_command to generate
  gps.pb.h/gps.pb.cc at build time; replaces fragile ../build/gps path hack
- Add GPS_PB_CC to DDP_FILES so the proto source is compiled into the lib
- Add if(CATKIN_ENABLE_TESTING) block with catkin_add_gtest targets for all
  three unit tests; set GPS_PROTO_GEN_DIR on include path

.github/workflows/ci.yml:
- cpp-build: replace broken ros-humble+catkin_make+||true job with
  container: ros:noetic-robot (Ubuntu 20.04 + ROS 1 Noetic), installs PR2
  packages and CPU LibTorch via pip, runs catkin_make + run_tests +
  catkin_test_results (no || true, real failure detection)
- tsan: replace broken job with standalone cmake build of test/ using
  clang-15 + -fsanitize=thread, runs ctest -V on the three unit tests

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

Author: robotsorcerer

.github/workflows/ci.yml

@@ -176,45 +176,66 @@ jobs:
                || true   # non-blocking until mypy baseline is clean
 
   # =========================================================================
-  # 4. C++ build with -Werror (catkin / CMake)
+  # 5. C++ build + unit tests — ROS Noetic (official ros:noetic-robot image)
+  #    Uses the canonical ROS 1 Docker image so catkin_make, the PR2 packages,
+  #    and the ROS dependency tree all match the target runtime environment.
+  #    CPU LibTorch is installed via pip; TORCH_ROOT locates the cmake config.
   # =========================================================================
   cpp-build:
-    name: C++17 build
+    name: C++17 / ROS Noetic build + tests
     runs-on: ubuntu-22.04
+    container:
+      image: ros:noetic-robot
 
     steps:
       - uses: actions/checkout@v4
 
-      - name: Install ROS 2 (for catkin_make) and build tools
+      - name: Install build dependencies
         run: |
-          sudo apt-get update
-          sudo apt-get install -y \
-            cmake build-essential \
-            libboost-all-dev \
+          apt-get update -q
+          DEBIAN_FRONTEND=noninteractive apt-get install -y --no-install-recommends \
+            python3-pip \
+            libprotobuf-dev protobuf-compiler \
             libeigen3-dev \
-            protobuf-compiler libprotobuf-dev \
-            ros-humble-desktop \
-            python3-catkin-tools || true
-          # Source ROS environment
-          source /opt/ros/humble/setup.bash || true
-
-      - name: Build with catkin
+            libgtest-dev \
+            libboost-system-dev \
+            ros-noetic-pr2-controller-interface \
+            ros-noetic-pr2-controller-manager \
+            ros-noetic-pr2-controllers \
+            ros-noetic-pr2-mechanism-model \
+            ros-noetic-control-toolbox \
+            ros-noetic-realtime-tools \
+            ros-noetic-orocos-kdl \
+            ros-noetic-pluginlib \
+            ros-noetic-tf
+
+      - name: Install CPU LibTorch
+        run: pip3 install --upgrade pip && pip3 install torch --index-url https://download.pytorch.org/whl/cpu
+
+      - name: Build catkin workspace
         run: |
-          source /opt/ros/humble/setup.bash || true
-          mkdir -p /tmp/catkin_ws/src
-          cp -r gps_agent_pkg /tmp/catkin_ws/src/
-          cd /tmp/catkin_ws
-          catkin_make \
-            -DCMAKE_CXX_STANDARD=17 \
-            -DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra" \
-            2>&1 | tee build.log
-          grep -q "Error" build.log && exit 1 || true
+          . /opt/ros/noetic/setup.sh
+          export TORCH_ROOT=$(python3 -c "import torch, os; print(os.path.dirname(torch.__file__))")
+          mkdir -p /ros_ws/src
+          cp -r $GITHUB_WORKSPACE/gps_agent_pkg /ros_ws/src/
+          cd /ros_ws
+          catkin_make -DCMAKE_BUILD_TYPE=RelWithDebInfo
+
+      - name: Run C++ unit tests
+        run: |
+          . /opt/ros/noetic/setup.sh
+          export TORCH_ROOT=$(python3 -c "import torch, os; print(os.path.dirname(torch.__file__))")
+          cd /ros_ws
+          catkin_make run_tests
+          catkin_test_results
 
   # =========================================================================
-  # 5. ThreadSanitizer (TSan) — detects C++ data races
+  # 6. ThreadSanitizer (TSan) — standalone test build, no ROS / LibTorch
+  #    Only the three unit tests under gps_agent_pkg/test/ are compiled.
+  #    LibTorch is intentionally excluded: it is incompatible with TSan.
   # =========================================================================
   tsan:
-    name: ThreadSanitizer
+    name: ThreadSanitizer (C++ data-race detection)
     runs-on: ubuntu-22.04
     needs: cpp-build
 
@@ -223,26 +244,32 @@ jobs:
 
       - name: Install build tools
         run: |
-          sudo apt-get update
-          sudo apt-get install -y cmake build-essential \
-            libboost-all-dev libeigen3-dev \
-            protobuf-compiler libprotobuf-dev \
-            clang-15
+          sudo apt-get update -q
+          sudo apt-get install -y --no-install-recommends \
+            cmake build-essential \
+            clang-15 \
+            libeigen3-dev \
+            libgtest-dev
 
-      - name: Build with TSan
+      - name: Build standalone tests with TSan
         run: |
-          mkdir -p /tmp/tsan_build && cd /tmp/tsan_build
-          cp -r $GITHUB_WORKSPACE/gps_agent_pkg .
-          cmake gps_agent_pkg \
+          mkdir -p /tmp/tsan_build
+          cd /tmp/tsan_build
+          cmake $GITHUB_WORKSPACE/gps_agent_pkg/test \
             -DCMAKE_CXX_STANDARD=17 \
             -DCMAKE_CXX_FLAGS="-fsanitize=thread -g -O1" \
             -DCMAKE_EXE_LINKER_FLAGS="-fsanitize=thread" \
             -DCMAKE_C_COMPILER=clang-15 \
-            -DCMAKE_CXX_COMPILER=clang++-15 || true
-          make -j$(nproc) 2>&1 | tail -50
+            -DCMAKE_CXX_COMPILER=clang++-15
+          make -j$(nproc)
+
+      - name: Run tests under TSan
+        run: |
+          cd /tmp/tsan_build
+          ctest -V
 
   # =========================================================================
-  # 6. Soak test (nightly only — NOT on every push)
+  # 7. Soak test (nightly only — NOT on every push)
   # =========================================================================
   soak-nightly:
     name: Soak test (nightly)

gps_agent_pkg/CMakeLists.txt

@@ -29,12 +29,43 @@ endif()
 find_package(Torch REQUIRED)
 message(STATUS "Found Torch: ${TORCH_INCLUDE_DIRS}")
 
-find_package(PkgConfig)
+# ---------------------------------------------------------------------------
+# Eigen
+# ---------------------------------------------------------------------------
+find_package(PkgConfig REQUIRED)
 pkg_search_module(Eigen3 REQUIRED eigen3)
 
 # ---------------------------------------------------------------------------
-# ROS version detection — ROS_VERSION env var set by ROS 1 (=1) and ROS 2 (=2)
-# Default to ROS 1 when unset so existing CI is unaffected.
+# Protobuf — generate gps.pb.h / gps.pb.cc at build time so the build is
+# self-contained and does not depend on a pre-built proto output tree.
+#
+# Headers are included as:  #include "gps/proto/gps.pb.h"
+# With GPS_PROTO_GEN_DIR on the include path, this resolves to:
+#   ${GPS_PROTO_GEN_DIR}/gps/proto/gps.pb.h
+# ---------------------------------------------------------------------------
+find_package(Protobuf REQUIRED)
+
+set(GPS_PROTO_GEN_DIR  ${CMAKE_CURRENT_BINARY_DIR}/proto_gen)
+set(GPS_PB_H           ${GPS_PROTO_GEN_DIR}/gps/proto/gps.pb.h)
+set(GPS_PB_CC          ${GPS_PROTO_GEN_DIR}/gps/proto/gps.pb.cc)
+
+add_custom_command(
+    OUTPUT  ${GPS_PB_H} ${GPS_PB_CC}
+    COMMAND ${CMAKE_COMMAND} -E make_directory ${GPS_PROTO_GEN_DIR}/gps/proto
+    COMMAND ${Protobuf_PROTOC_EXECUTABLE}
+            --proto_path=${CMAKE_CURRENT_SOURCE_DIR}/proto
+            --cpp_out=${GPS_PROTO_GEN_DIR}/gps/proto
+            ${CMAKE_CURRENT_SOURCE_DIR}/proto/gps.proto
+    DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/proto/gps.proto
+    COMMENT "Generating gps.pb.{h,cc} from proto/gps.proto"
+)
+
+# Suppress warnings in auto-generated protobuf code.
+set_source_files_properties(${GPS_PB_CC} PROPERTIES COMPILE_FLAGS "-w")
+
+# ---------------------------------------------------------------------------
+# ROS version detection — ROS_VERSION env var: 1 = ROS 1, 2 = ROS 2.
+# Default to ROS 1 when unset so existing catkin builds are unaffected.
 # ---------------------------------------------------------------------------
 if("$ENV{ROS_VERSION}" STREQUAL "2")
     set(GPS_ROS2_BUILD ON)
@@ -58,7 +89,7 @@ if(GPS_ROS2_BUILD)
     find_package(tf2_ros REQUIRED)
     find_package(rosidl_default_generators REQUIRED)
 
-    # ── Generate ROS 2 interfaces (all messages + UpdatePolicy service) ──
+    # Generate ROS 2 interfaces (all messages + UpdatePolicy service).
     # All GPS message types use only built-in field types or cross-references
     # within this package — no PR2-specific message dependencies.
     rosidl_generate_interfaces(${PROJECT_NAME}
@@ -79,12 +110,10 @@ if(GPS_ROS2_BUILD)
         DEPENDENCIES geometry_msgs sensor_msgs std_msgs
     )
 
-    # ── Portable C++ sources (no PR2 / pluginlib dependencies) ──────────
-    # PR2-specific files (pr2plugin.cpp, camerasensor.cpp, encodersensor.cpp,
-    # encoderfilter.cpp, rostopicsensor.cpp) depend on PR2 controller headers
-    # that don't exist in ROS 2.  The core PyTorch controller and algorithm
-    # logic are fully portable.
+    # Portable C++ sources (no PR2 / pluginlib dependencies).
+    # PR2-specific files depend on PR2 controller headers not in ROS 2.
     set(GPS_ROS2_SOURCES
+        ${GPS_PB_CC}
         src/robotplugin.cpp
         src/sample.cpp
         src/sensor.cpp
@@ -105,10 +134,14 @@ if(GPS_ROS2_BUILD)
     )
     target_include_directories(gps_agent_lib PUBLIC
         include
+        ${GPS_PROTO_GEN_DIR}
         ${TORCH_INCLUDE_DIRS}
-        $ENV{GPS_ROOT_DIR}/build/gps
     )
-    target_link_libraries(gps_agent_lib pthread ${TORCH_LIBRARIES})
+    target_link_libraries(gps_agent_lib
+        pthread
+        ${TORCH_LIBRARIES}
+        ${Protobuf_LIBRARIES}
+    )
     set_property(TARGET gps_agent_lib PROPERTY POSITION_INDEPENDENT_CODE ON)
 
     rosidl_target_interfaces(gps_agent_lib ${PROJECT_NAME} "rosidl_typesupport_cpp")
@@ -123,7 +156,7 @@ if(GPS_ROS2_BUILD)
     ament_package()
 
 # ===========================================================================
-# ROS 1 / catkin build path  (unchanged from original)
+# ROS 1 / catkin build path
 # ===========================================================================
 else()
 
@@ -156,8 +189,6 @@ else()
         tf
     )
 
-    include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../build/gps)
-
     find_package(Boost REQUIRED COMPONENTS system)
 
     ## Generate messages in the 'msg' folder
@@ -191,21 +222,23 @@ else()
     )
 
     catkin_package(
-        #  CATKIN_DEPENDS message_runtime control_toolbox geometry_msgs ...
+        CATKIN_DEPENDS message_runtime roscpp sensor_msgs geometry_msgs std_msgs
     )
 
     ###########
     ## Build ##
     ###########
-    include_directories(include)
 
     include_directories(
+        include
+        ${GPS_PROTO_GEN_DIR}      # generated gps.pb.h lives here
         ${catkin_INCLUDE_DIRS}
         ${TORCH_INCLUDE_DIRS}
-        $ENV{GPS_ROOT_DIR}/build/gps
+        ${Protobuf_INCLUDE_DIRS}
     )
 
     set(DDP_FILES
+        ${GPS_PB_CC}              # auto-generated from proto/gps.proto
         src/robotplugin.cpp
         src/pr2plugin.cpp
         src/sample.cpp
@@ -226,11 +259,69 @@ else()
 
     add_library(gps_agent_lib ${DDP_FILES})
 
-    target_link_libraries(gps_agent_lib pthread ${TORCH_LIBRARIES})
+    target_link_libraries(gps_agent_lib
+        pthread
+        ${TORCH_LIBRARIES}
+        ${catkin_LIBRARIES}
+        ${Protobuf_LIBRARIES}
+    )
 
     # LibTorch requires position-independent code when linking into a shared lib.
     set_property(TARGET gps_agent_lib PROPERTY POSITION_INDEPENDENT_CODE ON)
 
     add_dependencies(gps_agent_lib ${PROJECT_NAME}_gencpp)
 
+    ##########
+    ## Test ##
+    ##########
+
+    if(CATKIN_ENABLE_TESTING)
+        find_package(GTest REQUIRED)
+
+        # test_options_variant: exercises OptionsVariant (std::variant) and
+        # OptionsMap — the central C++17 parameter-passing type.
+        # No ROS or LibTorch symbols needed — options.h is self-contained.
+        catkin_add_gtest(test_options_variant
+            test/test_options_variant.cpp
+        )
+        target_include_directories(test_options_variant PRIVATE
+            include
+            ${catkin_INCLUDE_DIRS}   # provides Eigen include path
+            ${GTEST_INCLUDE_DIRS}
+        )
+        target_link_libraries(test_options_variant
+            ${GTEST_LIBRARIES}
+            pthread
+        )
+
+        # test_util: exercises util::split() and to_string<T>() — pure stdlib.
+        catkin_add_gtest(test_util
+            test/test_util.cpp
+            src/util.cpp
+        )
+        target_include_directories(test_util PRIVATE
+            include
+            ${GTEST_INCLUDE_DIRS}
+        )
+        target_link_libraries(test_util
+            ${GTEST_LIBRARIES}
+            pthread
+        )
+
+        # test_obs_normalization: tests the pointwise affine transform in
+        # PyTorchController::get_action() — only Eigen + gtest required.
+        catkin_add_gtest(test_obs_normalization
+            test/test_obs_normalization.cpp
+        )
+        target_include_directories(test_obs_normalization PRIVATE
+            ${catkin_INCLUDE_DIRS}   # provides Eigen include path
+            ${GTEST_INCLUDE_DIRS}
+        )
+        target_link_libraries(test_obs_normalization
+            ${GTEST_LIBRARIES}
+            pthread
+        )
+
+    endif() # CATKIN_ENABLE_TESTING
+
 endif() # GPS_ROS2_BUILD