syncronize camera_info with images

.docker/Dockerfile

@@ -45,6 +45,7 @@ RUN apt-get -q update && \
     apt-get -q -y dist-upgrade && \
     # Install common dependencies
     DEBIAN_FRONTEND=noninteractive apt-get -q install --no-install-recommends -y \
+        libgles2-mesa-dev libosmesa6-dev libglfw3-dev \
         curl git sudo python3-vcstool \
         $(test "${ROS_DISTRO}" = "noetic" && echo "python3-catkin-tools" || echo "python3-colcon-common-extensions") \
         clang clang-format clang-tidy clang-tools \

.github/workflows/ci.yaml

@@ -21,6 +21,7 @@ jobs:
       fail-fast: false
       matrix:
         distro: [noetic, one]
+        render_backend: [USE_GLFW, USE_OSMESA, USE_EGL]
         mujoco: [3.2.0]
         include:
           - distro: noetic
@@ -32,6 +33,15 @@ jobs:
             mujoco: 3.2.0
             env:
               CLANG_TIDY: pedantic
+        exclude:
+          - distro: one
+            render_backend: USE_EGL # requires GPU
+          - distro: one
+            render_backend: USE_GLFW # requires Xvfb
+          - distro: noetic
+            render_backend: USE_GLFW
+          - distro: noetic
+            render_backend: USE_EGL
 
     env:
       BUILDER: colcon
@@ -55,6 +65,7 @@ jobs:
       TARGET_CMAKE_ARGS: >
         -DCMAKE_BUILD_TYPE=${{ matrix.env.CCOV && 'Debug' || 'Release'}}
         -DCMAKE_CXX_FLAGS="-Werror $CXXFLAGS${{ matrix.env.CCOV && ' --coverage -O2 -fno-omit-frame-pointer'}}"
+        -DRENDER_BACKEND=${{ matrix.render_backend }}
       UPSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS= -DCMAKE_CXX_STANDARD=17
 
       CCACHE_DIR: ${{ github.workspace }}/.ccache
@@ -69,7 +80,7 @@ jobs:
       CATKIN_LINT: ${{ matrix.env.CATKIN_LINT || 'false' }}
       CCOV: ${{ matrix.env.CCOV || 'false' }}
 
-    name: "${{ matrix.distro }} mj-${{ matrix.mujoco }}${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }}"
+    name: "${{ matrix.distro }} mj-${{ matrix.mujoco }} ${{ matrix.render_backend }} ${{ matrix.env.CATKIN_LINT && ' + catkin_lint' || ''}}${{ matrix.env.CCOV && ' + ccov' || ''}}${{ matrix.env.CLANG_TIDY && (github.event_name != 'workflow_dispatch' && ' + clang-tidy (delta)' || ' + clang-tidy (all)') || '' }}"
     runs-on: ubuntu-20.04
     steps:
       - uses: actions/checkout@v4
@@ -114,7 +125,7 @@ jobs:
         uses: actions/upload-artifact@v4
         if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
         with:
-          name: test-results-${{ matrix.distro }}
+          name: test-results-${{ matrix.distro }}-${{ matrix.render_backend }}
           path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
       - name: Generate codecov report
         uses: rhaschke/lcov-action@main
@@ -123,11 +134,12 @@ jobs:
           docker: $DOCKER_IMAGE
           workdir: ${{ env.BASEDIR }}/target_ws
           ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
+          output: ${{ env.BASEDIR }}/target_ws/coverage-${{ matrix.distro }}-${{ matrix.render_backend }}.info
       - name: Upload codecov report
         uses: codecov/codecov-action@v5
         if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0'
         with:
-          files: ${{ env.BASEDIR }}/target_ws/coverage.info
+          files: ${{ env.BASEDIR }}/target_ws/coverage-${{ matrix.distro }}-${{ matrix.render_backend }}.info
       - name: Upload clang-tidy changes
         uses: rhaschke/upload-git-patch-action@main
         if: always() && matrix.env.CLANG_TIDY

CHANGELOG.md

@@ -20,12 +20,16 @@ Loading and reset times are reported in the server debug log. All plugin stats c
 * Re-added services for getting and setting gravity, that somehow vanished.
 * Fixed flaky tests that did not consider control callbacks being called in paused mode, too (#40).
 * Fixed bug that would not allow breaking out of *as fast as possible* stepping in headless mode without shutting down the simulation.
+* Fixed occasional segfault when offscreen context was freed on shutdown.
+* Fixed segmented image never being rendered/published.
 
 ### Changed
 * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h).
 * Increased test coverage of `mujoco_ros_sensors` plugin.
 * Split monolithic ros interface tests into more individual tests.
 * Added sleeping at least until the next lowerbound GUI refresh when paused to reduce cpu load.
+* deprecated `no_x` launchparameter in favor of using `no_render`, as offscreen rendering now is also available without X.
+* Optimized camera render configurations where RGB, Segmented and Depth streams are active, but only Segmented and Depth are subscribed. Previously, this would result in two separate low-level render calls, now it's done in one.
 
 Contributors: @DavidPL1
 

mujoco_ros/CMakeLists.txt

@@ -18,6 +18,12 @@ endif()
 set(OpenGL_GL_PREFERENCE GLVND)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+# Ensure generated header path exists both with catkin and colcon
+# catkin defines CATKIN_DEVEL_PREFIX, colcon does not define it
+if(NOT DEFINED CATKIN_DEVEL_PREFIX)
+ set(CATKIN_DEVEL_PREFIX ${CMAKE_CURRENT_BINARY_DIR})
+endif()
+
 list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
 include(ProjectOption)
 
@@ -60,7 +66,6 @@ configure_project_option(
 
 # Find MuJoCo
 find_package(mujoco 3.2.0 REQUIRED)
-find_library(GLFW libglfw.so.3)
 
 # ###############################################
 # # Declare ROS dynamic reconfigure parameters ##
@@ -80,6 +85,7 @@ find_library(GLFW libglfw.so.3)
 generate_dynamic_reconfigure_options(
   cfg/SimParams.cfg
 )
+include(ConfigureRenderBackend)
 
 # ##################################
 # # catkin specific configuration ##
@@ -116,6 +122,8 @@ add_subdirectory(src)
 
 # Depend on gencfg to ensure build before lib
 add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+# Depend on render_backend_h to ensure build before lib
+add_dependencies(${PROJECT_NAME} render_backend_h)
 
 # ############
 # # Install ##

mujoco_ros/cmake/ConfigureRenderBackend.cmake

@@ -0,0 +1,79 @@
+include_guard()
+
+find_library(GLFW libglfw.so.3) # Find GLFW3 for GUI
+
+set(RENDER_BACKEND "ANY" CACHE STRING "Choose rendering backend")
+set_property(CACHE RENDER_BACKEND PROPERTY STRINGS "ANY" "USE_GLFW" "USE_EGL" "USE_OSMESA")
+message(message "configured RENDER_BACKEND: ${RENDER_BACKEND}")
+
+if (RENDER_BACKEND STREQUAL "ANY")
+  unset(NO_GLFW)
+  unset(NO_EGL)
+  unset(NO_OSMESA)
+endif()
+
+if (RENDER_BACKEND STREQUAL "USE_GLFW")
+  set(NO_EGL ON)
+  set(NO_OSMESA ON)
+  message(STATUS "EGL and OSMESA disabled!")
+endif()
+
+if (RENDER_BACKEND STREQUAL "USE_EGL")
+  set(NO_GLFW ON)
+  message(STATUS "GLFW disabled! Will use OSMesa as fallback if EGL can not be found.")
+endif()
+
+if (RENDER_BACKEND STREQUAL "USE_OSMESA")
+  set(NO_GLFW ON)
+  set(NO_EGL ON)
+  message(STATUS "GLFW and EGL disabled!")
+endif()
+
+if (NO_GLFW OR ${GLFW} STREQUAL "GLFW-NOTFOUND")
+  message(WARNING "GLFW3 not found or disabled. GUI will not be available.")
+
+  find_package(OpenGL COMPONENTS OpenGL EGL) # Find OpenGL (EGL) for offscreen rendering
+  if (NO_EGL OR ${OpenGL_EGL_FOUND} STREQUAL "FALSE")
+    message(WARNING "EGL not found or disabled. Falling back to OSMESA.")
+
+    find_package(OSMesa)
+
+    if (NO_OSMESA OR !OSMesa_FOUND)
+      message(WARNING "EGL disabled or not found and OSMesa could not be found. Offscreen rendering will not be available!")
+      set(RENDERING_BACKEND "USE_NONE")
+    else() # OSMesa found
+      set(RENDERING_BACKEND "USE_OSMESA")
+      message(STATUS "OSMesa found. Offscreen rendering available.")
+    endif()
+
+  else() # EGL found
+    set(RENDERING_BACKEND "USE_EGL")
+    message(STATUS "EGL found. Offscreen rendering available.")
+  endif()
+
+else() # GLFW found
+  set(RENDERING_BACKEND "USE_GLFW")
+  message(STATUS "GLFW3 found. GUI and offscreen rendering available.")
+endif()
+
+add_custom_command(
+  OUTPUT ${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/render_backend.h always_rebuild
+  COMMAND ${CMAKE_COMMAND}
+  -DRENDER_BACKEND=${RENDERING_BACKEND}
+  -DHEADER_FILE_PATH=${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}
+  -P ${CMAKE_CURRENT_SOURCE_DIR}/cmake/GenerateBackendHeader.cmake
+  WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+)
+add_custom_target(render_backend_h
+ DEPENDS always_rebuild
+)
+
+list(APPEND ${PROJECT_NAME}_INCLUDE_DIRS
+  ${CATKIN_DEVEL_PREFIX}/include
+)
+
+# Install header file
+# catkin_lint: ignore_once external_file
+install(FILES ${CATKIN_DEVEL_PREFIX}/include/${PROJECT_NAME}/render_backend.h
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)

mujoco_ros/cmake/FindOSMesa.cmake

@@ -0,0 +1,60 @@
+#########
+# Taken from https://github.com/Kitware/VTK/blob/master/CMake/FindOSMesa.cmake
+#########
+
+# Try to find Mesa off-screen library and include dir.
+# Once done this will define
+#
+# OSMesa_FOUND        - true if OSMesa has been found
+# OSMesa_INCLUDE_DIRS - where the GL/osmesa.h can be found
+# OSMesa_LIBRARIES    - Link this to use OSMesa
+# OSMesa_VERSION      - Version of OSMesa found
+# OSMesa::OSMesa      - Imported target
+
+find_path(OSMESA_INCLUDE_DIR
+  NAMES GL/osmesa.h
+  PATHS "${OSMESA_ROOT}/include"
+        "$ENV{OSMESA_ROOT}/include"
+        /usr/openwin/share/include
+        /opt/graphics/OpenGL/include
+  DOC   "OSMesa include directory")
+mark_as_advanced(OSMESA_INCLUDE_DIR)
+
+find_library(OSMESA_LIBRARY
+  NAMES OSMesa OSMesa16 OSMesa32
+  PATHS "${OSMESA_ROOT}/lib"
+        "$ENV{OSMESA_ROOT}/lib"
+        /opt/graphics/OpenGL/lib
+        /usr/openwin/lib
+  DOC   "OSMesa library")
+mark_as_advanced(OSMESA_LIBRARY)
+
+if (OSMESA_INCLUDE_DIR AND EXISTS "${OSMESA_INCLUDE_DIR}/GL/osmesa.h")
+  file(STRINGS "${OSMESA_INCLUDE_DIR}/GL/osmesa.h" _OSMesa_version_lines
+    REGEX "OSMESA_[A-Z]+_VERSION")
+  string(REGEX REPLACE ".*# *define +OSMESA_MAJOR_VERSION +([0-9]+).*" "\\1" _OSMesa_version_major "${_OSMesa_version_lines}")
+  string(REGEX REPLACE ".*# *define +OSMESA_MINOR_VERSION +([0-9]+).*" "\\1" _OSMesa_version_minor "${_OSMesa_version_lines}")
+  string(REGEX REPLACE ".*# *define +OSMESA_PATCH_VERSION +([0-9]+).*" "\\1" _OSMesa_version_patch "${_OSMesa_version_lines}")
+  set(OSMesa_VERSION "${_OSMesa_version_major}.${_OSMesa_version_minor}.${_OSMesa_version_patch}")
+  unset(_OSMesa_version_major)
+  unset(_OSMesa_version_minor)
+  unset(_OSMesa_version_patch)
+  unset(_OSMesa_version_lines)
+endif ()
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(OSMesa
+  REQUIRED_VARS OSMESA_INCLUDE_DIR OSMESA_LIBRARY
+  VERSION_VAR OSMesa_VERSION)
+
+if (OSMesa_FOUND)
+  set(OSMesa_INCLUDE_DIRS "${OSMESA_INCLUDE_DIR}")
+  set(OSMesa_LIBRARIES "${OSMESA_LIBRARY}")
+
+  if (NOT TARGET OSMesa::OSMesa)
+    add_library(OSMesa::OSMesa UNKNOWN IMPORTED)
+    set_target_properties(OSMesa::OSMesa PROPERTIES
+      IMPORTED_LOCATION "${OSMESA_LIBRARY}"
+      INTERFACE_INCLUDE_DIRECTORIES "${OSMESA_INCLUDE_DIR}")
+  endif ()
+endif ()

mujoco_ros/cmake/Findmujoco.cmake

@@ -22,7 +22,7 @@ if(NOT mujoco_FOUND)
 	# Find dependencies
 	cmake_policy(SET CMP0072 NEW)
 	include(CMakeFindDependencyMacro)
-	find_dependency(OpenGL REQUIRED)
+	find_package(OpenGL)
 
 	if(mujoco_INCLUDE_DIRS AND mujoco_LIBRARIES)
 		set(mujoco_FOUND TRUE)

mujoco_ros/cmake/GenerateBackendHeader.cmake

@@ -0,0 +1,6 @@
+# Generate header file with rendering backend definition
+file(MAKE_DIRECTORY ${HEADER_FILE_PATH})
+configure_file(
+  ${CMAKE_CURRENT_SOURCE_DIR}/cmake/render_backend.h.in
+  ${HEADER_FILE_PATH}/render_backend.h
+)

mujoco_ros/cmake/render_backend.h.in

@@ -0,0 +1,8 @@
+#ifndef RENDER_BACKEND
+#define USE_GLFW 3
+#define USE_EGL 2
+#define USE_OSMESA 1
+#define USE_NONE 0
+
+#define RENDER_BACKEND ${RENDER_BACKEND}
+#endif

mujoco_ros/include/mujoco_ros/common_types.h

@@ -41,8 +41,6 @@
 #include <ros/ros.h>
 #include <image_transport/image_transport.h>
 
-#include <GLFW/glfw3.h>
-
 namespace mujoco_ros {
 
 using Clock = std::chrono::steady_clock;

mujoco_ros/include/mujoco_ros/mujoco_env.h

@@ -55,6 +55,7 @@
 
 #include <boost/thread.hpp>
 
+#include <mujoco_ros/render_backend.h>
 #include <mujoco_ros/common_types.h>
 #include <mujoco_ros/viewer.h>
 #include <mujoco_ros/plugin_utils.h>
@@ -90,8 +91,12 @@
 
 #include <rosgraph_msgs/Clock.h>
 
+#if RENDER_BACKEND == USE_GLFW
 #include <mujoco_ros/glfw_adapter.h>
 #include <mujoco_ros/glfw_dispatch.h>
+#elif RENDER_BACKEND == USE_OSMESA
+#include <GL/osmesa.h>
+#endif
 
 namespace mujoco_ros {
 
@@ -116,7 +121,16 @@ struct OffscreenRenderContext
 	mjvCamera cam;
 	std::unique_ptr<unsigned char[]> rgb;
 	std::unique_ptr<float[]> depth;
+#if RENDER_BACKEND == USE_GLFW
 	std::shared_ptr<GLFWwindow> window;
+#elif RENDER_BACKEND == USE_OSMESA
+	struct
+	{
+		OSMesaContext ctx;
+		unsigned char buffer[10000000]; // TODO: size necessary or resize later?
+		bool initialized = false;
+	} osmesa;
+#endif
 	mjrContext con = {};
 	mjvScene scn   = {};
 
@@ -268,7 +282,13 @@ class MujocoEnv
 
 	bool togglePaused(bool paused, const std::string &admin_hash = std::string());
 
+#if RENDER_BACKEND == USE_GLFW
 	GlfwAdapter *gui_adapter_ = nullptr;
+#endif
+
+#if RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA
+	bool InitGL();
+#endif
 
 	void runRenderCbs(mjvScene *scene);
 	bool step(int num_steps = 1, bool blocking = true);
@@ -323,9 +343,9 @@ class MujocoEnv
 
 	void notifyGeomChanged(const int geom_id);
 
+	boost::recursive_mutex sim_params_mutex_;
 	dynamic_reconfigure::Server<mujoco_ros::SimParamsConfig> *param_server_;
 	mujoco_ros::SimParamsConfig sim_params_;
-	boost::recursive_mutex sim_params_mutex_;
 	void dynparamCallback(mujoco_ros::SimParamsConfig &config, uint32_t level);
 	void updateDynamicParams();