diff --git a/.docker/Dockerfile b/.docker/Dockerfile index e87feb83..2ceaa40f 100644 --- a/.docker/Dockerfile +++ b/.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 \ diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1da027ed..ff54530f 100644 --- a/.github/workflows/ci.yaml +++ b/.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 diff --git a/CHANGELOG.md b/CHANGELOG.md index a43364c8..ca46d0d7 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/mujoco_ros/CMakeLists.txt b/mujoco_ros/CMakeLists.txt index d809cfbc..e1f97ace 100644 --- a/mujoco_ros/CMakeLists.txt +++ b/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 ## diff --git a/mujoco_ros/cmake/ConfigureRenderBackend.cmake b/mujoco_ros/cmake/ConfigureRenderBackend.cmake new file mode 100644 index 00000000..642413bf --- /dev/null +++ b/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} +) diff --git a/mujoco_ros/cmake/FindOSMesa.cmake b/mujoco_ros/cmake/FindOSMesa.cmake new file mode 100644 index 00000000..2b800130 --- /dev/null +++ b/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 () diff --git a/mujoco_ros/cmake/Findmujoco.cmake b/mujoco_ros/cmake/Findmujoco.cmake index 653f2591..6b4d1e9f 100644 --- a/mujoco_ros/cmake/Findmujoco.cmake +++ b/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) diff --git a/mujoco_ros/cmake/GenerateBackendHeader.cmake b/mujoco_ros/cmake/GenerateBackendHeader.cmake new file mode 100644 index 00000000..dc6084cc --- /dev/null +++ b/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 +) diff --git a/mujoco_ros/cmake/render_backend.h.in b/mujoco_ros/cmake/render_backend.h.in new file mode 100644 index 00000000..a6d94d4f --- /dev/null +++ b/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 diff --git a/mujoco_ros/include/mujoco_ros/common_types.h b/mujoco_ros/include/mujoco_ros/common_types.h index edf8546c..b486125e 100644 --- a/mujoco_ros/include/mujoco_ros/common_types.h +++ b/mujoco_ros/include/mujoco_ros/common_types.h @@ -41,8 +41,6 @@ #include #include -#include - namespace mujoco_ros { using Clock = std::chrono::steady_clock; diff --git a/mujoco_ros/include/mujoco_ros/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 0f02a633..9fed622a 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -55,6 +55,7 @@ #include +#include #include #include #include @@ -90,8 +91,12 @@ #include +#if RENDER_BACKEND == USE_GLFW #include #include +#elif RENDER_BACKEND == USE_OSMESA +#include +#endif namespace mujoco_ros { @@ -116,7 +121,16 @@ struct OffscreenRenderContext mjvCamera cam; std::unique_ptr rgb; std::unique_ptr depth; +#if RENDER_BACKEND == USE_GLFW std::shared_ptr 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 *param_server_; mujoco_ros::SimParamsConfig sim_params_; - boost::recursive_mutex sim_params_mutex_; void dynparamCallback(mujoco_ros::SimParamsConfig &config, uint32_t level); void updateDynamicParams(); diff --git a/mujoco_ros/include/mujoco_ros/offscreen_camera.h b/mujoco_ros/include/mujoco_ros/offscreen_camera.h index e10ba397..69a9dab3 100644 --- a/mujoco_ros/include/mujoco_ros/offscreen_camera.h +++ b/mujoco_ros/include/mujoco_ros/offscreen_camera.h @@ -53,10 +53,11 @@ namespace mujoco_ros::rendering { class OffscreenCamera { public: - OffscreenCamera(const uint8_t cam_id, const std::string &cam_name, const int width, const int height, - const streamType stream_type, const bool use_segid, const float pub_freq, - image_transport::ImageTransport *it, const ros::NodeHandle &parent_nh, const mjModel *model, - mjData *data, mujoco_ros::MujocoEnv *env_ptr); + OffscreenCamera(const uint8_t cam_id, const std::string &base_topic, const std::string &rgb_topic, + const std::string &depth_topic, const std::string &segment_topic, const std::string &cam_name, + const int width, const int height, const streamType stream_type, const bool use_segid, + const float pub_freq, const ros::NodeHandle &parent_nh, const mjModel *model, mjData *data, + mujoco_ros::MujocoEnv *env_ptr); ~OffscreenCamera() { @@ -66,11 +67,14 @@ class OffscreenCamera rgb_pub_.shutdown(); depth_pub_.shutdown(); segment_pub_.shutdown(); - camera_info_pub_.shutdown(); + rgb_camera_info_pub_.shutdown(); + depth_camera_info_pub_.shutdown(); + segment_camera_info_pub_.shutdown(); }; uint8_t cam_id_; std::string cam_name_; + std::string topic_; int width_, height_; streamType stream_type_ = streamType::RGB; bool use_segid_ = true; @@ -82,10 +86,14 @@ class OffscreenCamera mjvSceneState scn_state_; // Update depends on vopt, so every CamStream needs one ros::Time last_pub_; - ros::Publisher camera_info_pub_; + ros::NodeHandle nh_; + image_transport::ImageTransport it_; image_transport::Publisher rgb_pub_; + ros::Publisher rgb_camera_info_pub_; image_transport::Publisher depth_pub_; + ros::Publisher depth_camera_info_pub_; image_transport::Publisher segment_pub_; + ros::Publisher segment_camera_info_pub_; void renderAndPublish(mujoco_ros::OffscreenRenderContext *offscreen); @@ -98,8 +106,6 @@ class OffscreenCamera private: std::unique_ptr camera_info_manager_; - void publishCameraInfo(); - bool renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext *offscreen, const bool rgb, const bool depth, const bool segment); diff --git a/mujoco_ros/launch/launch_server.launch b/mujoco_ros/launch/launch_server.launch index 5a240639..47db6987 100644 --- a/mujoco_ros/launch/launch_server.launch +++ b/mujoco_ros/launch/launch_server.launch @@ -7,7 +7,8 @@ - + + @@ -45,6 +46,7 @@ + @@ -61,6 +63,7 @@ + @@ -80,6 +83,7 @@ + @@ -99,6 +103,7 @@ + diff --git a/mujoco_ros/src/CMakeLists.txt b/mujoco_ros/src/CMakeLists.txt index 9b0dabe4..8d57e7ff 100644 --- a/mujoco_ros/src/CMakeLists.txt +++ b/mujoco_ros/src/CMakeLists.txt @@ -9,30 +9,32 @@ target_include_directories(lodepng PRIVATE target_link_libraries(lodepng PRIVATE project_option) add_library(mujoco_ros::lodepng ALIAS lodepng) -add_library(platform_ui_adapter OBJECT - $ - glfw_adapter.cc - glfw_dispatch.cc - platform_ui_adapter.cc -) -set_property(TARGET platform_ui_adapter PROPERTY POSITION_INDEPENDENT_CODE ON) -target_include_directories(platform_ui_adapter PUBLIC - ${PROJECT_SOURCE_DIR}/include - ${GLFW_INTERFACE_INCLUDE_DIRECTORIES} -) -target_link_libraries(platform_ui_adapter - PUBLIC - mujoco::mujoco - ${GLFW} - PRIVATE - mujoco_ros::lodepng - project_option - project_warning -) -add_library(mujoco_ros::platform_ui_adapter ALIAS platform_ui_adapter) +if(RENDERING_BACKEND STREQUAL "USE_GLFW") + add_library(platform_ui_adapter OBJECT + $ + glfw_adapter.cc + glfw_dispatch.cc + platform_ui_adapter.cc + ) + set_property(TARGET platform_ui_adapter PROPERTY POSITION_INDEPENDENT_CODE ON) + target_include_directories(platform_ui_adapter PUBLIC + ${PROJECT_SOURCE_DIR}/include + ${GLFW_INTERFACE_INCLUDE_DIRECTORIES} + ) + target_link_libraries(platform_ui_adapter + PUBLIC + mujoco::mujoco + ${GLFW} + PRIVATE + mujoco_ros::lodepng + project_option + project_warning + ) + add_library(mujoco_ros::platform_ui_adapter ALIAS platform_ui_adapter) +endif() add_library(${PROJECT_NAME} - $ + $ mujoco_env.cpp viewer.cpp plugin_utils.cpp @@ -42,6 +44,14 @@ add_library(${PROJECT_NAME} physics.cpp ) +if(RENDERING_BACKEND STREQUAL "USE_GLFW") + target_sources(${PROJECT_NAME} PRIVATE $) +endif() + +if (RENDERING_BACKEND) + target_compile_definitions(${PROJECT_NAME} PUBLIC ${RENDERING_BACKEND}=1) +endif() + target_include_directories(${PROJECT_NAME} PUBLIC ${PROJECT_SOURCE_DIR}/include ${mujoco_include_DIRS} @@ -53,14 +63,20 @@ target_include_directories(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} PUBLIC mujoco::mujoco - mujoco_ros::platform_ui_adapter catkin_pkg PRIVATE - mujoco_ros::lodepng project_option project_warning ) +if(RENDERING_BACKEND STREQUAL "USE_GLFW") + target_link_libraries(${PROJECT_NAME} PUBLIC mujoco_ros::platform_ui_adapter) +elseif(RENDERING_BACKEND STREQUAL "USE_EGL") + target_link_libraries(${PROJECT_NAME} PUBLIC OpenGL::EGL) +elseif(RENDERING_BACKEND STREQUAL "USE_OSMESA") + target_link_libraries(${PROJECT_NAME} PUBLIC OSMesa::OSMesa) +endif() + # Node Executable add_executable(mujoco_node main.cpp diff --git a/mujoco_ros/src/main.cpp b/mujoco_ros/src/main.cpp index 547c9a66..2b606813 100644 --- a/mujoco_ros/src/main.cpp +++ b/mujoco_ros/src/main.cpp @@ -34,8 +34,8 @@ /* Authors: David P. Leins */ -#include -#include +#include + #include #include @@ -45,6 +45,11 @@ #include #include +#if RENDER_BACKEND == USE_GLFW +#include +#include +#endif + namespace { std::unique_ptr env; @@ -138,13 +143,6 @@ int main(int argc, char **argv) // TODO(dleins): Should MuJoCo Plugins be loaded? env = std::make_unique(admin_hash); - bool no_x; - nh.param("no_x", no_x, false); - - if (!no_x) { - nh.param("headless", no_x, false); - } - // const char *filename = nullptr; if (!filename.empty()) { mju::strcpy_arr(env->queued_filename_, filename.c_str()); @@ -154,19 +152,19 @@ int main(int argc, char **argv) env->startPhysicsLoop(); env->startEventLoop(); - if (!no_x) { - // mjvCamera cam; - // mjvOption opt; - // mjv_defaultCamera(&cam); - // mjv_defaultOption(&opt); +#if RENDER_BACKEND == USE_GLFW + if (!env->settings_.headless) { ROS_INFO("Launching viewer"); - auto viewer = - // std::make_unique(std::unique_ptr(env->gui_adapter_), - // env.get(), &cam, &opt, /* is_passive = */ false); - std::make_unique(std::unique_ptr(env->gui_adapter_), - env.get(), /* is_passive = */ false); + auto viewer = std::make_unique( + std::unique_ptr(env->gui_adapter_), env.get(), /* is_passive = */ false); viewer->RenderLoop(); - } else { + } +#else + if (!env->settings_.headless) { + ROS_ERROR("GLFW backend not available. Cannot launch viewer"); + } +#endif + else { ROS_INFO("Running headless"); } diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index e63494b7..40cfd0fd 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -43,12 +43,23 @@ #include #include +#if RENDER_BACKEND == USE_GLFW +static std::string render_backend = "GLFW"; +#elif RENDER_BACKEND == USE_OSMESA +static std::string render_backend = "OSMESA"; +#elif RENDER_BACKEND == USE_EGL +static std::string render_backend = "EGL"; +#else +static std::string render_backend = "NONE. No offscreen rendering available."; +#endif + namespace mujoco_ros { namespace mju = ::mujoco::sample_util; using Seconds = std::chrono::duration; using Milliseconds = std::chrono::duration; +#if RENDER_BACKEND == USE_GLFW namespace { int MaybeGlfwInit() { @@ -62,6 +73,7 @@ int MaybeGlfwInit() return is_initialized; } } // namespace +#endif MujocoEnv *MujocoEnv::instance = nullptr; @@ -75,15 +87,29 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) } ROS_DEBUG_COND(!settings_.use_sim_time, "use_sim_time is set to false. Not publishing sim time to /clock!"); + bool no_render; nh_ = std::make_unique("~"); ROS_DEBUG_STREAM("New MujocoEnv created"); + nh_->param("no_render", no_render, false); + if (nh_->hasParam("no_x")) { + ROS_WARN("The 'no_x' parameter is deprecated. Use 'no_render' instead."); + nh_->param("no_x", no_render, no_render); + } + + if (no_render) { + ROS_INFO("no_render is set. Disabling rendering and setting headless to true"); + nh_->setParam("headless", true); + nh_->setParam("render_offscreen", false); + } + ROS_INFO("Using MuJoCo library version %s", mj_versionString()); if (mjVERSION_HEADER != mj_version()) { ROS_WARN_STREAM("Headers and library have different versions (headers: " << mjVERSION_HEADER << ", library: " << mj_version() << ")"); } + ROS_INFO_STREAM("Compiled with render backend: " << render_backend); if (!admin_hash.empty()) { mju::strcpy_arr(settings_.admin_hash, admin_hash.c_str()); @@ -104,20 +130,18 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) ROS_INFO("Running in normal training mode."); } - bool no_x; nh_->param("render_offscreen", settings_.render_offscreen, true); - nh_->param("no_x", no_x, false); - - if (no_x && settings_.render_offscreen) { - ROS_WARN("no_x implies offscreen is disabled! Disabling offscreen rendering."); - settings_.render_offscreen = false; + if (settings_.render_offscreen) { + mjv_makeScene(nullptr, &offscreen_.scn, Viewer::kMaxGeom); } - bool headless = true; - nh_->param("headless", headless, true); - if (!headless) { - mjv_makeScene(nullptr, &offscreen_.scn, Viewer::kMaxGeom); + nh_->param("headless", settings_.headless, true); + if (!settings_.headless) { +#if RENDER_BACKEND == USE_GLFW gui_adapter_ = new mujoco_ros::GlfwAdapter(); +#else + ROS_ERROR("Compiled without GLFW support. Cannot run in non-headless mode."); +#endif } if (settings_.use_sim_time) { @@ -134,9 +158,23 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) mjv_defaultPerturb(&pert_); if (settings_.render_offscreen) { - MaybeGlfwInit(); - ROS_DEBUG("Starting offscreen render thread"); - offscreen_.render_thread_handle = boost::thread(&MujocoEnv::offscreenRenderLoop, this); + bool can_render = true; + +#if RENDER_BACKEND == USE_GLFW + can_render = MaybeGlfwInit(); + ROS_ERROR_COND(!can_render, "Failed to initialize GLFW. Cannot render offscreen!"); +#elif RENDER_BACKEND == NONE + ROS_ERROR("No rendering backend available. Cannot render offscreen!"); + can_render = false; +#endif + + if (!can_render) { + settings_.render_offscreen = false; + ROS_ERROR("Disabling offscreen rendering"); + } else { + ROS_DEBUG("Starting offscreen render thread"); + offscreen_.render_thread_handle = boost::thread(&MujocoEnv::offscreenRenderLoop, this); + } } nh_->param("num_steps", num_steps_until_exit_, -1); @@ -212,7 +250,7 @@ void MujocoEnv::eventLoop() is_event_running_ = 1; auto now = Clock::now(); auto fps_cap = Seconds(mujoco_ros::Viewer::render_ui_rate_upper_bound_); // Cap at 60 fps - while (ros::ok() && !settings_.exit_request.load() && (!settings_.headless)) { + while (ros::ok() && !settings_.exit_request.load()) { { std::unique_lock lock(physics_thread_mutex_); now = Clock::now(); @@ -569,6 +607,9 @@ void MujocoEnv::loadWithModelAndData() model_.reset(mnew, mj_deleteModel); data_.reset(dnew, mj_deleteData); + // perform a forward pass to initialize all fields if not done yet (very important for offscreen rendering) + mj_forward(model_.get(), data_.get()); + if (model_->opt.integrator == mjINT_EULER) { ROS_WARN("Euler integrator detected. Euler is default for legacy reasons, consider using implicitfast, which is " "recommended for most applications."); diff --git a/mujoco_ros/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index 47b34bc9..c2bd10d8 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -47,20 +47,24 @@ namespace mujoco_ros::rendering { -OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_name, const int width, const int height, +OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &base_topic, const std::string &rgb_topic, + const std::string &depth_topic, const std::string &segment_topic, + const std::string &cam_name, const int width, const int height, const streamType stream_type, const bool use_segid, const float pub_freq, - image_transport::ImageTransport *it, const ros::NodeHandle &parent_nh, - const mjModel *model, mjData *data, mujoco_ros::MujocoEnv *env_ptr) + const ros::NodeHandle &parent_nh, const mjModel *model, mjData *data, + mujoco_ros::MujocoEnv *env_ptr) : cam_id_(cam_id) , cam_name_(cam_name) + , topic_(base_topic) , width_(width) , height_(height) , stream_type_(stream_type) , use_segid_(use_segid) , pub_freq_(pub_freq) + , nh_(parent_nh, base_topic) + , it_(nh_) { last_pub_ = ros::Time::now(); - ros::NodeHandle nh(parent_nh, "cameras/" + cam_name); mjv_defaultOption(&vopt_); mjv_defaultSceneState(&scn_state_); @@ -68,15 +72,18 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na if (stream_type & streamType::RGB) { ROS_DEBUG_NAMED("mujoco_env", "\tCreating rgb publisher"); - rgb_pub_ = it->advertise("cameras/" + cam_name + "/rgb", 1); + rgb_pub_ = it_.advertise(rgb_topic + "/image_raw", 1); + rgb_camera_info_pub_ = nh_.advertise(rgb_topic + "/camera_info", 1, true); } if (stream_type & streamType::DEPTH) { ROS_DEBUG_NAMED("mujoco_env", "\tCreating depth publisher"); - depth_pub_ = it->advertise("cameras/" + cam_name + "/depth", 1); + depth_pub_ = it_.advertise(depth_topic + "/image_raw", 1); + depth_camera_info_pub_ = nh_.advertise(depth_topic + "/camera_info", 1, true); } if (stream_type & streamType::SEGMENTED) { ROS_DEBUG_NAMED("mujoco_env", "\tCreating segmentation publisher"); - segment_pub_ = it->advertise("cameras/" + cam_name + "/segmented", 1); + segment_pub_ = it_.advertise(segment_topic + "/image_raw", 1); + segment_camera_info_pub_ = nh_.advertise(segment_topic + "/camera_info", 1, true); } ROS_DEBUG_STREAM_NAMED("mujoco_env", "\tSetting up camera stream(s) of type '" @@ -120,7 +127,7 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na env_ptr->registerStaticTransform(cam_transform); // init camera info manager - camera_info_manager_ = std::make_unique(nh, cam_name); + camera_info_manager_ = std::make_unique(nh_, cam_name); // Get camera info mjtNum cam_pos[3]; @@ -153,7 +160,6 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na mju_copy(ci.K.c_array() + 6, extrinsic + 8, 3); camera_info_manager_->setCameraInfo(ci); - camera_info_pub_ = nh.advertise("camera_info", 1, true); } bool OffscreenCamera::shouldRender(const ros::Time &t) @@ -165,11 +171,9 @@ bool OffscreenCamera::shouldRender(const ros::Time &t) bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext *offscreen, const bool rgb, const bool depth, const bool segment) { - if ((!rgb && !depth) || // nothing to render - (rgb_pub_.getNumSubscribers() == 0 && depth_pub_.getNumSubscribers() == 0) || // no subscribers - (!rgb && depth_pub_.getNumSubscribers() == 0) || - (!depth && ((rgb_pub_.getNumSubscribers() == 0 && !segment) || // would render depth but no depth subscribers - (segment && segment_pub_.getNumSubscribers() == 0)))) { // would render rgb but no rgb subscribers + bool rgb_or_s = rgb || segment; + + if (!rgb_or_s && !depth) { // Nothing to render return false; } @@ -185,20 +189,27 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext // Render to buffer mjr_render(viewport, &offscreen->scn, &offscreen->con); // read buffers - if (rgb && depth) { + if (rgb_or_s && depth) { mjr_readPixels(offscreen->rgb.get(), offscreen->depth.get(), viewport, &offscreen->con); - } else if (rgb) { + } else if (rgb_or_s) { mjr_readPixels(offscreen->rgb.get(), nullptr, viewport, &offscreen->con); - } else if (depth) { + } else { // depth only mjr_readPixels(nullptr, offscreen->depth.get(), viewport, &offscreen->con); } +#if RENDER_BACKEND == USE_GLFW glfwSwapBuffers(offscreen->window.get()); +#endif + + // create info msg + auto ros_time = ros::Time(scn_state_.data.time); + sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo(); + camera_info_msg.header.stamp = ros_time; - if (rgb) { + if (rgb_or_s) { // Publish RGB image sensor_msgs::ImagePtr rgb_msg = boost::make_shared(); rgb_msg->header.frame_id = cam_name_ + "_optical_frame"; - rgb_msg->header.stamp = ros::Time(scn_state_.data.time); + rgb_msg->header.stamp = ros_time; rgb_msg->width = static_castwidth)>(viewport.width); rgb_msg->height = static_castheight)>(viewport.height); rgb_msg->encoding = sensor_msgs::image_encodings::RGB8; @@ -216,8 +227,10 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext if (segment) { segment_pub_.publish(rgb_msg); + segment_camera_info_pub_.publish(camera_info_msg); } else { rgb_pub_.publish(rgb_msg); + rgb_camera_info_pub_.publish(camera_info_msg); } } @@ -225,7 +238,7 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext // Publish DEPTH image sensor_msgs::ImagePtr depth_msg = boost::make_shared(); depth_msg->header.frame_id = cam_name_ + "_optical_frame"; - depth_msg->header.stamp = ros::Time(scn_state_.data.time); + depth_msg->header.stamp = ros_time; depth_msg->width = util::as_unsigned(viewport.width); depth_msg->height = util::as_unsigned(viewport.height); depth_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; @@ -249,7 +262,9 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext } depth_pub_.publish(depth_msg); + depth_camera_info_pub_.publish(camera_info_msg); } + return true; } @@ -261,54 +276,29 @@ void OffscreenCamera::renderAndPublish(mujoco_ros::OffscreenRenderContext *offsc initial_published_ = true; - last_pub_ = ros::Time(scn_state_.data.time); - bool rendered = false; + last_pub_ = ros::Time(scn_state_.data.time); - bool segment = stream_type_ & streamType::SEGMENTED; - bool rgb = stream_type_ & streamType::RGB; - bool depth = stream_type_ & streamType::DEPTH; + bool segment = (stream_type_ & streamType::SEGMENTED) && + (segment_pub_.getNumSubscribers() > 0 || segment_camera_info_pub_.getNumSubscribers() > 0); + bool rgb = (stream_type_ & streamType::RGB) && + (rgb_pub_.getNumSubscribers() > 0 || rgb_camera_info_pub_.getNumSubscribers() > 0); + bool depth = (stream_type_ & streamType::DEPTH) && + (depth_pub_.getNumSubscribers() > 0 || depth_camera_info_pub_.getNumSubscribers() > 0); offscreen->cam.fixedcamid = cam_id_; - // Render RGB and DEPTH image - if (rgb && depth) { - offscreen->scn.flags[mjRND_SEGMENT] = 0; - rendered = renderAndPubIfNecessary(offscreen, true, true, false); - - // Render segmentation image additionally to RGB and DEPTH - if (segment) { - offscreen->scn.flags[mjRND_SEGMENT] = 1; - rendered = rendered || renderAndPubIfNecessary(offscreen, true, false, true); - } - } else if (segment && depth) { // DEPTH and SEGMENTED - offscreen->scn.flags[mjRND_SEGMENT] = 1; - rendered = renderAndPubIfNecessary(offscreen, true, true, true); - } else if (rgb && segment) { // RGB and SEGMENTED - // Needs two calls, because both go into the rgb buffer + // If rgb and segment are requested, we need to run two low-level render passes (segment is rgb with a different + // flag). + if (rgb && segment) { // first render RGB and maybe DEPTH, then SEGMENTED offscreen->scn.flags[mjRND_SEGMENT] = 0; - rendered = renderAndPubIfNecessary(offscreen, true, false, false); + renderAndPubIfNecessary(offscreen, true, depth, false); offscreen->scn.flags[mjRND_SEGMENT] = 1; - rendered = rendered || renderAndPubIfNecessary(offscreen, true, false, true); - } else if (rgb) { // RGB only - offscreen->scn.flags[mjRND_SEGMENT] = 0; - rendered = renderAndPubIfNecessary(offscreen, true, false, false); - } else { // Only DEPTH or SEGMENTED - offscreen->scn.flags[mjRND_SEGMENT] = 1; - rendered = renderAndPubIfNecessary(offscreen, segment, true, segment); + renderAndPubIfNecessary(offscreen, false, false, true); + } else { // render maybe RGB/SEGMENTED and maybe DEPTH + offscreen->scn.flags[mjRND_SEGMENT] = segment; + renderAndPubIfNecessary(offscreen, rgb, depth, segment); } - - if (rendered) { - publishCameraInfo(); - } -} - -void OffscreenCamera::publishCameraInfo() -{ - sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo(); - camera_info_msg.header.stamp = ros::Time::now(); - - camera_info_pub_.publish(camera_info_msg); } } // namespace mujoco_ros::rendering diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 9bd204ac..f1ed9bce 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -40,24 +40,58 @@ #include +#if RENDER_BACKEND == USE_EGL +#include +#endif + namespace mujoco_ros { OffscreenRenderContext::~OffscreenRenderContext() { +#if RENDER_BACKEND == USE_GLFW if (window != nullptr) { - // Making context current before calling mjr_freeContext causes a bad access error - // Let glfwTerminate() handle the cleanup then - ROS_DEBUG("Freeing offscreen context"); + ROS_DEBUG("Freeing GLFW offscreen context"); + std::unique_lock lock(render_mutex); + request_pending.store(false); + mjr_defaultContext(&con); mjr_freeContext(&con); } +#elif RENDER_BACKEND == USE_EGL + ROS_DEBUG("Freeing EGL offscreen context"); + mjr_defaultContext(&con); + mjr_freeContext(&con); + EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (display != EGL_NO_DISPLAY) { + // Get current context + EGLContext current_context = eglGetCurrentContext(); + + // Release context + eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT); + + // Destroy context if valid + if (current_context != EGL_NO_CONTEXT) { + eglDestroyContext(display, current_context); + } + + // Terminate display + eglTerminate(display); + } +#elif RENDER_BACKEND == USE_OSMESA + if (!osmesa.initialized) { + return; + } + ROS_DEBUG("Freeing OSMESA offscreen context"); + mjr_defaultContext(&con); + mjr_freeContext(&con); + OSMesaDestroyContext(osmesa.ctx); +#endif } void MujocoEnv::initializeRenderResources() { - image_transport::ImageTransport it(*nh_); bool config_exists, use_segid; rendering::streamType stream_type; - std::string cam_name, cam_config_path; + std::string cam_name, cam_config_path, base_topic, rgb, depth, segment; float pub_freq; int max_res_h = 0, max_res_w = 0; @@ -92,19 +126,31 @@ void MujocoEnv::initializeRenderResources() res_w_string += "/width"; std::string res_h_string(param_path); res_h_string += "/height"; + std::string base_topic_string(param_path); + base_topic_string += "/topic"; + std::string rgb_topic_string(param_path); + rgb_topic_string += "/name_rgb"; + std::string depth_topic_string(param_path); + depth_topic_string += "/name_depth"; + std::string segment_topic_string(param_path); + segment_topic_string += "/name_segment"; stream_type = rendering::streamType(this->nh_->param(stream_type_string, rendering::streamType::RGB)); pub_freq = this->nh_->param(pub_freq_string, 15); use_segid = this->nh_->param(segid_string, true); res_w = this->nh_->param(res_w_string, 720); res_h = this->nh_->param(res_h_string, 480); + base_topic = this->nh_->param(base_topic_string, "cameras/" + cam_name); + rgb = this->nh_->param(rgb_topic_string, "rgb"); + depth = this->nh_->param(depth_topic_string, "depth"); + segment = this->nh_->param(segment_topic_string, "segmented"); max_res_h = std::max(res_h, max_res_h); max_res_w = std::max(res_w, max_res_w); - offscreen_.cams.emplace_back(std::make_unique(cam_id, cam_name, res_w, res_h, - stream_type, use_segid, pub_freq, &it, - *nh_, model_.get(), data_.get(), this)); + offscreen_.cams.emplace_back(std::make_unique( + cam_id, base_topic, rgb, depth, segment, cam_name, res_w, res_h, stream_type, use_segid, pub_freq, *nh_, + model_.get(), data_.get(), this)); } if (model_->vis.global.offheight < max_res_h || model_->vis.global.offwidth < max_res_w) { @@ -121,9 +167,11 @@ void MujocoEnv::initializeRenderResources() ROS_DEBUG_NAMED("offscreen_rendering", "Initializing offscreen rendering utils"); +#if RENDER_BACKEND == USE_GLFW Glfw().glfwMakeContextCurrent(offscreen_.window.get()); // Glfw().glfwSetWindowSize(offscreen_.window.get(), max_res_w, max_res_h); glfwSetWindowSize(offscreen_.window.get(), max_res_w, max_res_h); +#endif mjr_makeContext(this->model_.get(), &offscreen_.con, 50); ROS_DEBUG_NAMED("offscreen_rendering", "\tApplied model to context"); @@ -131,9 +179,94 @@ void MujocoEnv::initializeRenderResources() mjr_setBuffer(mjFB_OFFSCREEN, &offscreen_.con); } +#if RENDER_BACKEND == USE_EGL +bool MujocoEnv::InitGL() +{ + ROS_DEBUG("Initializing EGL..."); + // clang-format off + const EGLint config[] = { + EGL_RED_SIZE, 8, + EGL_GREEN_SIZE, 8, + EGL_BLUE_SIZE, 8, + EGL_ALPHA_SIZE, 8, + EGL_DEPTH_SIZE, 24, + EGL_STENCIL_SIZE, 8, + EGL_COLOR_BUFFER_TYPE, EGL_RGB_BUFFER, + EGL_SURFACE_TYPE, EGL_PBUFFER_BIT, + EGL_RENDERABLE_TYPE, EGL_OPENGL_BIT, + EGL_NONE }; + // clang-format on + + // Get Display + EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY); + if (display == EGL_NO_DISPLAY) { + ROS_ERROR_STREAM("Failed to get EGL display. Error type: " << eglGetError()); + return false; + } + + // Initialize EGL + EGLint major, minor; + if (eglInitialize(display, &major, &minor) != EGL_TRUE) { + ROS_ERROR_STREAM("Failed to initialize EGL. Error type: " << eglGetError()); + return false; + } + + // Choose Config + EGLint num_configs; + EGLConfig egl_config; + if (eglChooseConfig(display, config, &egl_config, 1, &num_configs) != EGL_TRUE) { + ROS_ERROR_STREAM("Failed to choose EGL config. Error type: " << eglGetError()); + return false; + } + + // bind OpenGL API + if (eglBindAPI(EGL_OPENGL_API) != EGL_TRUE) { + ROS_ERROR_STREAM("Failed to bind OpenGL API. Error type: " << eglGetError()); + return false; + } + + // Create context + EGLContext context = eglCreateContext(display, egl_config, EGL_NO_CONTEXT, nullptr); + if (context == EGL_NO_CONTEXT) { + ROS_ERROR_STREAM("Failed to create EGL context. Error type: " << eglGetError()); + return false; + } + + // Make current + if (eglMakeCurrent(display, EGL_NO_SURFACE, EGL_NO_SURFACE, context) != EGL_TRUE) { + ROS_ERROR_STREAM("Failed to make EGL context current. Error type: " << eglGetError()); + return false; + } + + ROS_DEBUG("EGL initialized"); + return true; +} +#elif RENDER_BACKEND == USE_OSMESA +bool MujocoEnv::InitGL() +{ + ROS_DEBUG("Initializing OSMesa..."); + // Initialize OSMesa + offscreen_.osmesa.ctx = OSMesaCreateContextExt(GL_RGBA, 24, 8, 8, nullptr); + if (!offscreen_.osmesa.ctx) { + ROS_ERROR("OSMesa context creation failed"); + return false; + } + + // Make current + // if (!OSMesaMakeCurrent(offscreen_.osmesa.ctx, offscreen_.osmesa.buffer, GL_UNSIGNED_BYTE, width, height)) { + if (!OSMesaMakeCurrent(offscreen_.osmesa.ctx, offscreen_.osmesa.buffer, GL_UNSIGNED_BYTE, 800, 800)) { + ROS_ERROR("OSMesa make current failed"); + return false; + } + ROS_DEBUG("OSMesa initialized"); + offscreen_.osmesa.initialized = true; + return true; +} // InitGL +#endif + void MujocoEnv::offscreenRenderLoop() { - is_rendering_running_ = 1; +#if RENDER_BACKEND == USE_GLFW Glfw().glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_FALSE); Glfw().glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE); offscreen_.window.reset(Glfw().glfwCreateWindow(800, 600, "Invisible window", nullptr, nullptr), @@ -149,7 +282,22 @@ void MujocoEnv::offscreenRenderLoop() Glfw().glfwMakeContextCurrent(offscreen_.window.get()); Glfw().glfwSwapInterval(0); +#elif RENDER_BACKEND == USE_EGL + if (!InitGL()) { + ROS_ERROR("Failed to initialize EGL. Cannot run offscreen rendering"); + return; + } +#elif RENDER_BACKEND == USE_OSMESA + if (!InitGL()) { + ROS_ERROR("Failed to initialize OSMesa. Cannot run offscreen rendering"); + return; + } +#else + ROS_ERROR("No offscreen rendering backend available. Cannot run offscreen rendering"); + return; +#endif + is_rendering_running_ = 1; ROS_DEBUG_NAMED("offscreen_rendering", "Creating offscreen rendering resources ..."); mjv_defaultCamera(&offscreen_.cam); // Set to fixed camera @@ -172,8 +320,10 @@ void MujocoEnv::offscreenRenderLoop() // Wait for render request std::unique_lock lock(offscreen_.render_mutex); // ROS_DEBUG_NAMED("offscreen_rendering", "Waiting for render request"); - offscreen_.cond_render_request.wait( - lock, [this] { return offscreen_.request_pending.load() || settings_.visual_init_request.load(); }); + offscreen_.cond_render_request.wait(lock, [this] { + return offscreen_.request_pending.load() || settings_.visual_init_request.load() || + settings_.exit_request.load(); + }); // In case of exit request after waiting for render request if (!ros::ok() || settings_.exit_request.load()) { @@ -184,6 +334,7 @@ void MujocoEnv::offscreenRenderLoop() ROS_DEBUG_NAMED("offscreen_rendering", "Initializing render resources"); initializeRenderResources(); settings_.visual_init_request = false; + continue; } for (const auto &cam_ptr : offscreen_.cams) { diff --git a/mujoco_ros/src/physics.cpp b/mujoco_ros/src/physics.cpp index b225d637..cdbf4495 100644 --- a/mujoco_ros/src/physics.cpp +++ b/mujoco_ros/src/physics.cpp @@ -83,7 +83,6 @@ void MujocoEnv::physicsLoop() ROS_INFO_COND(num_steps_until_exit_ == 0, "Reached requested number of steps. Exiting simulation"); // settings_.exit_request.store(1); if (offscreen_.render_thread_handle.joinable()) { - offscreen_.request_pending.store(true); offscreen_.cond_render_request.notify_one(); ROS_DEBUG("Joining offscreen render thread"); offscreen_.render_thread_handle.join(); diff --git a/mujoco_ros/test/CMakeLists.txt b/mujoco_ros/test/CMakeLists.txt index e0fda585..6808f71a 100644 --- a/mujoco_ros/test/CMakeLists.txt +++ b/mujoco_ros/test/CMakeLists.txt @@ -51,7 +51,25 @@ target_link_libraries(mujoco_ros_plugin_test project_warning ) +add_rostest_gtest(mujoco_render_test + launch/mujoco_render.test + mujoco_render_test.cpp +) + +add_dependencies(mujoco_render_test + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) + +target_link_libraries(mujoco_render_test + mujoco_ros + project_option + project_warning +) + install(FILES empty_world.xml + equality_world.xml + camera_world.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/test ) diff --git a/mujoco_ros/test/camera_world.xml b/mujoco_ros/test/camera_world.xml new file mode 100644 index 00000000..da228177 --- /dev/null +++ b/mujoco_ros/test/camera_world.xml @@ -0,0 +1,45 @@ + + diff --git a/mujoco_ros/test/launch/mujoco_render.test b/mujoco_ros/test/launch/mujoco_render.test new file mode 100644 index 00000000..897a9232 --- /dev/null +++ b/mujoco_ros/test/launch/mujoco_render.test @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index 3934484e..6ce3c87a 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -63,6 +63,8 @@ class MujocoEnvTestWrapper : public MujocoEnv int isEventRunning() { return is_event_running_; } int isRenderingRunning() { return is_rendering_running_; } + OffscreenRenderContext *getOffscreenContext() { return &offscreen_; } + int getNumCBReadyPlugins() { return cb_ready_plugins_.size(); } void notifyGeomChange() { notifyGeomChanged(0); } @@ -81,12 +83,22 @@ class MujocoEnvTestWrapper : public MujocoEnv const std::string &getHandleNamespace() { return nh_->getNamespace(); } - void startWithXML(const std::string &xml_path) + void startWithXML(const std::string &xml_path, bool wait = true, float timeout_secs = 2.) { mju::strcpy_arr(queued_filename_, xml_path.c_str()); settings_.load_request = 2; startPhysicsLoop(); startEventLoop(); + + if (not wait) + return; + + // Wait for model to be loaded + float seconds = 0; + while (getOperationalStatus() != 0 && seconds < timeout_secs) { // wait for model to be loaded or timeout + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } } }; @@ -94,16 +106,24 @@ class BaseEnvFixture : public ::testing::Test { protected: std::unique_ptr nh; + std::unique_ptr env_ptr = nullptr; void SetUp() override { nh = std::make_unique("~"); nh->setParam("unpause", true); - nh->setParam("no_x", true); + nh->setParam("no_render", true); nh->setParam("use_sim_time", true); } - void TearDown() override {} + void TearDown() override + { + if (env_ptr != nullptr) { + env_ptr->shutdown(); + } + // clean up all parameters + ros::param::del(nh->getNamespace()); + } }; class PendulumEnvFixture : public ::testing::Test @@ -116,7 +136,7 @@ class PendulumEnvFixture : public ::testing::Test { nh = std::make_unique("~"); nh->setParam("unpause", false); - nh->setParam("no_x", true); + nh->setParam("no_render", true); nh->setParam("use_sim_time", true); nh->setParam("sim_steps", -1); @@ -158,7 +178,7 @@ class EqualityEnvFixture : public ::testing::Test { nh.reset(new ros::NodeHandle("~")); nh->setParam("unpause", false); - nh->setParam("no_x", true); + nh->setParam("no_render", true); nh->setParam("use_sim_time", true); nh->setParam("sim_steps", -1); diff --git a/mujoco_ros/test/mujoco_env_test.cpp b/mujoco_ros/test/mujoco_env_test.cpp index b7307075..a42a0506 100644 --- a/mujoco_ros/test/mujoco_env_test.cpp +++ b/mujoco_ros/test/mujoco_env_test.cpp @@ -60,352 +60,257 @@ TEST_F(BaseEnvFixture, EvalModeWithoutHashThrow) { nh->setParam("eval_mode", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - EXPECT_THROW(MujocoEnvTestWrapper env, std::runtime_error); - nh->setParam("eval_mode", false); + EXPECT_THROW(env_ptr = std::make_unique(""), std::runtime_error); } TEST_F(BaseEnvFixture, RunEvalMode) { nh->setParam("eval_mode", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env("some_hash"); + env_ptr = std::make_unique("some_hash"); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; - EXPECT_FALSE(env.settings_.exit_request) << "Exit request is set before shutdown!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!"; + EXPECT_FALSE(env_ptr->settings_.exit_request) << "Exit request is set before shutdown!"; - env.shutdown(); - nh->setParam("eval_mode", false); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, EvalPauseWithHash) { nh->setParam("eval_mode", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env("some_hash"); - - env.startWithXML(xml_path); - env.settings_.run = 1; - - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; - - env.togglePaused(true, "some_hash"); - EXPECT_FALSE(env.settings_.run) << "Model should not be running!"; - - env.shutdown(); - nh->setParam("eval_mode", false); -} - -TEST_F(BaseEnvFixture, EvalPauseWithoutHashForbidden) -{ - nh->setParam("eval_mode", true); - std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env("some_hash"); + env_ptr = std::make_unique("some_hash"); - env.startWithXML(xml_path); - env.settings_.run = 1; + env_ptr->startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!"; - env.togglePaused(true); - EXPECT_TRUE(env.settings_.run) << "Model should still be running!"; + env_ptr->togglePaused(true, "some_hash"); + EXPECT_FALSE(env_ptr->settings_.run) << "Model should not be running!"; - env.shutdown(); - nh->setParam("eval_mode", false); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, EvalUnpauseWithHash) { nh->setParam("eval_mode", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env("some_hash"); + env_ptr = std::make_unique("some_hash"); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!"; - env.togglePaused(false, "some_hash"); - EXPECT_TRUE(env.settings_.run) << "Model should be running!"; + env_ptr->togglePaused(false, "some_hash"); + EXPECT_TRUE(env_ptr->settings_.run) << "Model should be running!"; - env.shutdown(); - nh->setParam("eval_mode", false); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepBeforeLoad) { - MujocoEnvTestWrapper env; - EXPECT_FALSE(env.step(1)); + env_ptr = std::make_unique(""); + EXPECT_FALSE(env_ptr->step(1)); } TEST_F(BaseEnvFixture, StepAfterShutdown) { - MujocoEnvTestWrapper env; - env.shutdown(); - EXPECT_FALSE(env.step(1)); + env_ptr = std::make_unique(""); + env_ptr->shutdown(); + EXPECT_FALSE(env_ptr->step(1)); } TEST_F(BaseEnvFixture, StepWhileUnpaused) { std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, 2) << "Model was not loaded correctly!"; - EXPECT_FALSE(env.step(1)); + env_ptr->startWithXML(xml_path); + EXPECT_FALSE(env_ptr->step(1)); - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepSingleWhilePaused) { nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, 2) << "Model was not loaded correctly!"; - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); - EXPECT_TRUE(env.step(1)); - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, env.getModelPtr()->opt.timestep); + env_ptr->startWithXML(xml_path); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, 0.0); + EXPECT_TRUE(env_ptr->step(1)); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, env_ptr->getModelPtr()->opt.timestep); - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepMultiWhilePaused) { nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, 2) << "Model was not loaded correctly!"; - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); - EXPECT_TRUE(env.step(100)); - EXPECT_NEAR(env.getDataPtr()->time, 100 * env.getModelPtr()->opt.timestep, 1e-6); + env_ptr->startWithXML(xml_path); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, 0.0); + EXPECT_TRUE(env_ptr->step(100)); + EXPECT_NEAR(env_ptr->getDataPtr()->time, 100 * env_ptr->getModelPtr()->opt.timestep, 1e-6); - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepUnblocked) { nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, 2) << "Model was not loaded correctly!"; - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); - EXPECT_TRUE(env.step(100, false)); - EXPECT_GT(env.settings_.env_steps_request, 0); + env_ptr->startWithXML(xml_path); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, 0.0); + EXPECT_TRUE(env_ptr->step(100, false)); + EXPECT_GT(env_ptr->settings_.env_steps_request, 0); - seconds = 0; - while (env.getDataPtr()->time < 100 * env.getModelPtr()->opt.timestep && seconds < 2) { + float seconds = 0; + while (env_ptr->getDataPtr()->time < 100 * env_ptr->getModelPtr()->opt.timestep && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Time should have passed but ran into 2 seconds timeout!"; - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepNegativeFail) { nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, 2) << "Model was not loaded correctly!"; - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); - EXPECT_FALSE(env.step(-10)) << "Stepping with negative steps should not succeed!"; - EXPECT_EQ(env.settings_.env_steps_request, 0); - EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); - - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->startWithXML(xml_path); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, 0.0); + EXPECT_FALSE(env_ptr->step(-10)) << "Stepping with negative steps should not succeed!"; + EXPECT_EQ(env_ptr->settings_.env_steps_request, 0); + EXPECT_DOUBLE_EQ(env_ptr->getDataPtr()->time, 0.0); + + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, Shutdown) { - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - EXPECT_FALSE(env.isPhysicsRunning()) << "Physics thread should not be running yet!"; - EXPECT_FALSE(env.isEventRunning()) << "Event thread should not be running yet!"; + EXPECT_FALSE(env_ptr->isPhysicsRunning()) << "Physics thread should not be running yet!"; + EXPECT_FALSE(env_ptr->isEventRunning()) << "Event thread should not be running yet!"; - env.startPhysicsLoop(); - env.startEventLoop(); + env_ptr->startPhysicsLoop(); + env_ptr->startEventLoop(); - EXPECT_FALSE(env.settings_.exit_request) << "Exit request is set before shutdown!"; + EXPECT_FALSE(env_ptr->settings_.exit_request) << "Exit request is set before shutdown!"; // Make sure the threads are running float seconds = 0; - while (seconds < 2 && (!env.isPhysicsRunning() || !env.isEventRunning())) { // wait for threads to start + while (seconds < 2 && (!env_ptr->isPhysicsRunning() || !env_ptr->isEventRunning())) { // wait for threads to start std::this_thread::sleep_for(std::chrono::milliseconds(3)); seconds += 0.003; } - EXPECT_TRUE(env.isPhysicsRunning()) << "Physics thread should have started by now!"; - EXPECT_TRUE(env.isEventRunning()) << "Event thread should have started by now!"; + EXPECT_TRUE(env_ptr->isPhysicsRunning()) << "Physics thread should have started by now!"; + EXPECT_TRUE(env_ptr->isEventRunning()) << "Event thread should have started by now!"; - env.settings_.exit_request = 1; + env_ptr->settings_.exit_request = 1; seconds = 0; - while (seconds < 2 && (env.isPhysicsRunning() || env.isEventRunning())) { // wait for threads to exit + while (seconds < 2 && (env_ptr->isPhysicsRunning() || env_ptr->isEventRunning())) { // wait for threads to exit std::this_thread::sleep_for(std::chrono::milliseconds(3)); seconds += 0.003; } - EXPECT_FALSE(env.isPhysicsRunning()) << "Physics thread is still running after shutdown!"; - EXPECT_FALSE(env.isEventRunning()) << "Event thread is still running after shutdown!"; + EXPECT_FALSE(env_ptr->isPhysicsRunning()) << "Physics thread is still running after shutdown!"; + EXPECT_FALSE(env_ptr->isEventRunning()) << "Event thread is still running after shutdown!"; - env.waitForEventsJoin(); - env.waitForPhysicsJoin(); + env_ptr->waitForEventsJoin(); + env_ptr->waitForPhysicsJoin(); } TEST_F(BaseEnvFixture, InitWithModel) { std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!"; seconds = 0; - while (env.getDataPtr()->time == 0 && seconds < 2) { // wait for model to be loaded or timeout + while (env_ptr->getDataPtr()->time == 0 && seconds < 2) { // wait for model to be loaded or timeout std::this_thread::sleep_for(std::chrono::milliseconds(5)); seconds += 0.005; } EXPECT_LT(seconds, 2) << "Time did not pass in simulation, ran into 2 second timeout!"; - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, EvalUnpauseWithoutHash) { std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env("some_hash"); + env_ptr = std::make_unique("some_hash"); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_EQ(env.getFilename(), xml_path) << "Model was not loaded correctly!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Model was not loaded correctly!"; - env.togglePaused(false); - EXPECT_TRUE(env.settings_.run) << "Model should be running!"; + env_ptr->togglePaused(false); + EXPECT_TRUE(env_ptr->settings_.run) << "Model should be running!"; - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, PauseUnpause) { nh->setParam("unpause", false); - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - env.startWithXML(xml_path); - - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } + env_ptr->startWithXML(xml_path); - EXPECT_FALSE(env.settings_.run) << "Model should not be running!"; + EXPECT_FALSE(env_ptr->settings_.run) << "Model should not be running!"; - mjtNum time = env.getDataPtr()->time; + mjtNum time = env_ptr->getDataPtr()->time; std::this_thread::sleep_for(std::chrono::milliseconds(10)); - EXPECT_EQ(env.getDataPtr()->time, time) << "Time should not have changed in paused mode!"; + EXPECT_EQ(env_ptr->getDataPtr()->time, time) << "Time should not have changed in paused mode!"; - env.settings_.run.store(1); + env_ptr->settings_.run.store(1); - seconds = 0; - while (env.getDataPtr()->time == time && seconds < 2) { // wait for model to be loaded or timeout + float seconds = 0; + while (env_ptr->getDataPtr()->time == time && seconds < 2) { // wait for model to be loaded or timeout std::this_thread::sleep_for(std::chrono::milliseconds(5)); seconds += 0.005; } EXPECT_LT(seconds, 2) << "Time should have been moving forward in unpaused state, ran into 2 seconds timeout!"; - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, StepsTerminate) { nh->setParam("num_steps", 100); - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml"; - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() > 1 && seconds < 2) { // wait for model to be loaded or timeout - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - - seconds = 0; int current; - int last = env.getPendingSteps(); - while (env.getPendingSteps() > 0) { - current = env.getPendingSteps(); + int last = env_ptr->getPendingSteps(); + while (env_ptr->getPendingSteps() > 0) { + current = env_ptr->getPendingSteps(); if (current == last) { // wait for model to be loaded or timeout std::this_thread::sleep_for(std::chrono::milliseconds(2)); seconds += 0.002; @@ -418,114 +323,108 @@ TEST_F(BaseEnvFixture, StepsTerminate) } EXPECT_LT(seconds, 2) << "Pending steps should have decreased but ran into 2 seconds timeout"; - EXPECT_NEAR(env.getDataPtr()->time, env.getModelPtr()->opt.timestep * 100, env.getModelPtr()->opt.timestep * 0.1) + EXPECT_NEAR(env_ptr->getDataPtr()->time, env_ptr->getModelPtr()->opt.timestep * 100, + env_ptr->getModelPtr()->opt.timestep * 0.1) << "Time should have stopped after 100 steps"; - nh->deleteParam("num_steps"); - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, ManualSteps) { nh->setParam("unpause", false); - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml"; - env.startWithXML(xml_path); - - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } + env_ptr->startWithXML(xml_path); - EXPECT_FALSE(env.settings_.env_steps_request) << "pending manual steps should be 0 after initialization!"; - EXPECT_FALSE(env.settings_.run) << "Model should not be running!"; - EXPECT_EQ(env.getDataPtr()->time, 0) << "Time should be 0 after initialization!"; + EXPECT_FALSE(env_ptr->settings_.env_steps_request) << "pending manual steps should be 0 after initialization!"; + EXPECT_FALSE(env_ptr->settings_.run) << "Model should not be running!"; + EXPECT_EQ(env_ptr->getDataPtr()->time, 0) << "Time should be 0 after initialization!"; - env.settings_.env_steps_request.store(1); + env_ptr->settings_.env_steps_request.store(1); float seconds = 0; - while (env.settings_.env_steps_request != 0 && seconds < 1) { // wait for model to be loaded + while (env_ptr->settings_.env_steps_request != 0 && seconds < 1) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(2)); seconds += 0.002; } EXPECT_LT(seconds, 1) << "Manual step should have been executed but ran into 1 second timeout!"; - EXPECT_EQ(env.getDataPtr()->time, env.getModelPtr()->opt.timestep) << "Time should have been increased by one step!"; + EXPECT_EQ(env_ptr->getDataPtr()->time, env_ptr->getModelPtr()->opt.timestep) + << "Time should have been increased by one step!"; - env.settings_.run.store(1); - env.settings_.env_steps_request.store(100); + env_ptr->settings_.run.store(1); + env_ptr->settings_.env_steps_request.store(100); // Wait for time to pass std::this_thread::sleep_for(std::chrono::milliseconds(2)); - EXPECT_EQ(env.settings_.env_steps_request, 100) << "pending manual steps should not change in unpaused mode!"; - env.settings_.env_steps_request.store(0); - env.settings_.run.store(0); + EXPECT_EQ(env_ptr->settings_.env_steps_request, 100) << "pending manual steps should not change in unpaused mode!"; + env_ptr->settings_.env_steps_request.store(0); + env_ptr->settings_.run.store(0); - mjtNum time = env.getDataPtr()->time; + mjtNum time = env_ptr->getDataPtr()->time; - env.settings_.env_steps_request.store(100); + env_ptr->settings_.env_steps_request.store(100); seconds = 0; - while (env.settings_.env_steps_request != 0 && seconds < 2) { // wait for model to be loaded + while (env_ptr->settings_.env_steps_request != 0 && seconds < 2) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(5)); seconds += 0.005; } EXPECT_LT(seconds, 2) << "Manual step should have been executed but ran into 1 second timeout!"; - EXPECT_NEAR(env.getDataPtr()->time, time + 100 * env.getModelPtr()->opt.timestep, - env.getModelPtr()->opt.timestep * 0.1) + EXPECT_NEAR(env_ptr->getDataPtr()->time, time + 100 * env_ptr->getModelPtr()->opt.timestep, + env_ptr->getModelPtr()->opt.timestep * 0.1) << "Time should have been increased by 100*timestep!"; - env.shutdown(); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, Reset) { nh->setParam("unpause", false); - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml"; - env.startWithXML(xml_path); - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } + env_ptr->startWithXML(xml_path); - EXPECT_TRUE(env.step(100)) << "Stepping failed!"; + EXPECT_TRUE(env_ptr->step(100)) << "Stepping failed!"; - env.settings_.run = 0; - EXPECT_NEAR(env.getDataPtr()->time, 100 * env.getModelPtr()->opt.timestep, 1e-6) << "Time should have been running!"; + env_ptr->settings_.run = 0; + EXPECT_NEAR(env_ptr->getDataPtr()->time, 100 * env_ptr->getModelPtr()->opt.timestep, 1e-6) + << "Time should have been running!"; - env.settings_.reset_request.store(1); + env_ptr->settings_.reset_request.store(1); float seconds = 0; - while (env.settings_.reset_request != 0 && seconds < 2) { // wait for model to be loaded + while (env_ptr->settings_.reset_request != 0 && seconds < 2) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(2)); seconds += 0.002; } EXPECT_LT(seconds, 2) << "Reset should have been executed but ran into 2 seconds timeout!"; - EXPECT_FALSE(env.settings_.run) << "Model should stay paused after reset!"; - EXPECT_NEAR(env.getDataPtr()->time, 0, 1e-6) << "Time should have been reset to 0!"; + EXPECT_FALSE(env_ptr->settings_.run) << "Model should stay paused after reset!"; + EXPECT_NEAR(env_ptr->getDataPtr()->time, 0, 1e-6) << "Time should have been reset to 0!"; - env.settings_.run = 1; - env.settings_.reset_request.store(1); - while (env.settings_.reset_request != 0) { // wait for model to be loaded + env_ptr->settings_.run = 1; + env_ptr->settings_.reset_request.store(1); + while (env_ptr->settings_.reset_request != 0) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - EXPECT_TRUE(env.settings_.run) << "Model should keep running after reset!"; + EXPECT_TRUE(env_ptr->settings_.run) << "Model should keep running after reset!"; - env.settings_.run = 0; - int id2 = mujoco_ros::util::jointName2id(env.getModelPtr(), "joint2"); + env_ptr->settings_.run = 0; + int id2 = mujoco_ros::util::jointName2id(env_ptr->getModelPtr(), "joint2"); EXPECT_NE(id2, -1) << "joint2 should exist in model!"; - env.getDataPtr()->qpos[env.getModelPtr()->jnt_qposadr[id2]] = 0.5; - env.getDataPtr()->qvel[env.getModelPtr()->jnt_dofadr[id2]] = 0.1; - env.settings_.reset_request.store(1); - while (env.settings_.reset_request != 0) { // wait for model to be loaded + env_ptr->getDataPtr()->qpos[env_ptr->getModelPtr()->jnt_qposadr[id2]] = 0.5; + env_ptr->getDataPtr()->qvel[env_ptr->getModelPtr()->jnt_dofadr[id2]] = 0.1; + env_ptr->settings_.reset_request.store(1); + while (env_ptr->settings_.reset_request != 0) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(2)); } - EXPECT_NE(env.getDataPtr()->qpos[id2], 0.5) << "joint2 position should have been reset!"; - EXPECT_NE(env.getDataPtr()->qvel[id2], 0.1) << "joint2 velocity should have been reset!"; + EXPECT_NE(env_ptr->getDataPtr()->qpos[id2], 0.5) << "joint2 position should have been reset!"; + EXPECT_NE(env_ptr->getDataPtr()->qvel[id2], 0.1) << "joint2 velocity should have been reset!"; - env.shutdown(); + env_ptr->shutdown(); } // Test reloading @@ -533,99 +432,86 @@ TEST_F(BaseEnvFixture, Reload) { nh->setParam("unpause", false); - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - env.startWithXML(xml_path); - - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } + env_ptr->startWithXML(xml_path); // Load same model again in unpaused state - load_queued_model(env); - EXPECT_FALSE(env.settings_.run) << "Model should stay paused on init!"; - EXPECT_EQ(env.getFilename(), xml_path) << "Wrong content in filename_!"; - EXPECT_EQ(env.getDataPtr()->time, 0) << "Time should have been reset to 0!"; - EXPECT_EQ(env.settings_.run, 0) << "Model should stay paused after reset!"; + load_queued_model(env_ptr.get()); + EXPECT_FALSE(env_ptr->settings_.run) << "Model should stay paused on init!"; + EXPECT_EQ(env_ptr->getFilename(), xml_path) << "Wrong content in filename_!"; + EXPECT_EQ(env_ptr->getDataPtr()->time, 0) << "Time should have been reset to 0!"; + EXPECT_EQ(env_ptr->settings_.run, 0) << "Model should stay paused after reset!"; // Load new model in paused state std::string xml_path2 = ros::package::getPath("mujoco_ros") + "/test/pendulum_world.xml"; - mju::strcpy_arr(env.queued_filename_, xml_path2.c_str()); + mju::strcpy_arr(env_ptr->queued_filename_, xml_path2.c_str()); - load_queued_model(env); - EXPECT_EQ(env.getFilename(), xml_path2) << "Wrong content in filename_!"; + load_queued_model(env_ptr.get()); + EXPECT_EQ(env_ptr->getFilename(), xml_path2) << "Wrong content in filename_!"; - env.settings_.run.store(1); + env_ptr->settings_.run.store(1); // Let some time pass - while (env.getDataPtr()->time < 0.01) { + while (env_ptr->getDataPtr()->time < 0.01) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } // Load same model in unpaused state - load_queued_model(env); - EXPECT_EQ(env.getFilename(), xml_path2) << "Wrong content in filename_!"; + load_queued_model(env_ptr.get()); + EXPECT_EQ(env_ptr->getFilename(), xml_path2) << "Wrong content in filename_!"; // Let some time pass - while (env.getDataPtr()->time < 0.01) { + while (env_ptr->getDataPtr()->time < 0.01) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - env.shutdown(); - nh->setParam("unpause", false); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, InitModelFromQueuedBuffer) { // Create a MujocoEnv object - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); // Set the queued model buffer std::string queuedFilename = ""; // Call the initModelFromQueue function - env.startWithXML(queuedFilename); - - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } + env_ptr->startWithXML(queuedFilename); // Check the result - ASSERT_TRUE(env.getModelPtr()); - ASSERT_TRUE(env.getDataPtr()); - ASSERT_STREQ(env.getFilename().c_str(), queuedFilename.c_str()); - ASSERT_TRUE(env.sim_state_.model_valid); + ASSERT_TRUE(env_ptr->getModelPtr()); + ASSERT_TRUE(env_ptr->getDataPtr()); + ASSERT_STREQ(env_ptr->getFilename().c_str(), queuedFilename.c_str()); + ASSERT_TRUE(env_ptr->sim_state_.model_valid); - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, InitModelFromInvalidQueuedBuffer) { // Create a MujocoEnv object - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); // Set the queued model buffer std::string valid = ""; // Call the initModelFromQueue function - env.startWithXML(valid); - - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } + env_ptr->startWithXML(valid); std::string invalid = ""; - env.load_filename(invalid); + env_ptr->load_filename(invalid); - while (env.getOperationalStatus() != 0) { // wait for model to be loaded + while (env_ptr->getOperationalStatus() != 0) { // wait for model to be loaded std::this_thread::sleep_for(std::chrono::milliseconds(3)); } // Check the result - ASSERT_TRUE(env.getModelPtr()); - ASSERT_TRUE(env.getDataPtr()); - ASSERT_STREQ(env.getFilename().c_str(), valid.c_str()); - ASSERT_FALSE(env.sim_state_.model_valid); + ASSERT_TRUE(env_ptr->getModelPtr()); + ASSERT_TRUE(env_ptr->getDataPtr()); + ASSERT_STREQ(env_ptr->getFilename().c_str(), valid.c_str()); + ASSERT_FALSE(env_ptr->sim_state_.model_valid); - env.shutdown(); + env_ptr->shutdown(); } diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp new file mode 100644 index 00000000..53f78dc9 --- /dev/null +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -0,0 +1,994 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: David P. Leins */ + +#include + +#include "mujoco_env_fixture.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "mujoco_render_test"); + + // Uncomment to enable debug output (useful for debugging failing tests) + // ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + // ros::console::notifyLoggerLevelsChanged(); + + // Create spinner to communicate with ROS + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::NodeHandle nh; + int ret = RUN_ALL_TESTS(); + + // Stop spinner and shutdown ROS before returning + spinner.stop(); + ros::shutdown(); + return ret; +} + +using namespace mujoco_ros; +namespace mju = ::mujoco::sample_util; + +TEST_F(BaseEnvFixture, Not_Headless_Warn) +{ + nh->setParam("no_render", false); + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + while (env_ptr->getOperationalStatus() != 0) { // wait for model to be loaded + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } + + env_ptr->shutdown(); +} + +#if RENDER_BACKEND == USE_GLFW || RENDER_BACKEND == USE_EGL || \ + RENDER_BACKEND == USE_OSMESA // i.e. any render backend available +TEST_F(BaseEnvFixture, NoRender_Params_Correct) +{ + nh->setParam("no_render", true); + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + bool offscreen = true, headless = false; + nh->getParam("render_offscreen", offscreen); + nh->getParam("headless", headless); + EXPECT_TRUE(headless); + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_FALSE(offscreen); + EXPECT_FALSE(env_ptr->settings_.render_offscreen); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Headless_params_correct) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + EXPECT_TRUE(env_ptr->settings_.headless); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::RGB); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool img = false, info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + info = true; + } + if (img && info) + break; + } + EXPECT_TRUE(img && info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::DEPTH); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::DEPTH); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool img = false, info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + info = true; + } + if (img && info) + break; + } + EXPECT_TRUE(img && info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, SEGMENTATION_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::SEGMENTED); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::SEGMENTED); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool img = false, info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + info = true; + } + if (img && info) + break; + } + EXPECT_TRUE(img && info); + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_DEPTH_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::RGB_D); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool found_rgb = false, found_depth = false; + bool found_rgb_info = false, found_depth_info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; + } + if (found_rgb && found_depth && found_rgb_info && found_depth_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_rgb_info && found_depth_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_S); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::RGB_S); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool found_rgb = false, found_seg = false; + bool found_rgb_info = false, found_seg_info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { + found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_seg && found_rgb_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_seg && found_rgb_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::DEPTH_S); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::DEPTH_S); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool found_depth = false, found_seg = false; + bool found_depth_info = false, found_seg_info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { + found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; + } + if (found_depth && found_seg && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_depth && found_seg && found_depth_info && found_seg_info); + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topics_Available) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D_S); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_TRUE(offscreen->cams[0]->stream_type_ == rendering::streamType::RGB_D_S); + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/image_raw") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/image_raw") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/image_raw") { + found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Default_Cam_Settings) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + + // Check default camera settings + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->pub_freq_, 15); + EXPECT_EQ(offscreen->cams[0]->width_, 720); + EXPECT_EQ(offscreen->cams[0]->height_, 480); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Resolution_Settings) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/width", 640); + nh->setParam("cam_config/test_cam/height", 480); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + + // Check camera settings + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->pub_freq_, 15); + EXPECT_EQ(offscreen->cams[0]->width_, 640); + EXPECT_EQ(offscreen->cams[0]->height_, 480); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Stream_BaseTopic_Relative) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D_S); + nh->setParam("cam_config/test_cam/topic", "alt_topic"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_STREQ(offscreen->cams[0]->topic_.c_str(), "alt_topic"); + + bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/rgb/image_raw") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/depth/image_raw") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/segmented/image_raw") { + found_seg = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/depth/camera_info") { + found_depth_info = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/alt_topic/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Stream_BaseTopic_Absolute) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB_D_S); + nh->setParam("cam_config/test_cam/topic", "/alt_topic"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_STREQ(offscreen->cams[0]->topic_.c_str(), "/alt_topic"); + + bool found_rgb = false, found_depth = false, found_seg = false; + bool found_rgb_info = false, found_depth_info = false, found_seg_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == "/alt_topic/rgb/image_raw") { + found_rgb = true; + } else if (t.name == "/alt_topic/depth/image_raw") { + found_depth = true; + } else if (t.name == "/alt_topic/segmented/image_raw") { + found_seg = true; + } else if (t.name == "/alt_topic/rgb/camera_info") { + found_rgb_info = true; + } else if (t.name == "/alt_topic/depth/camera_info") { + found_depth_info = true; + } else if (t.name == "/alt_topic/segmented/camera_info") { + found_seg_info = true; + } + if (found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::RGB); + nh->setParam("cam_config/test_cam/name_rgb", "alt_rgb"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_rgb/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_rgb/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::DEPTH); + nh->setParam("cam_config/test_cam/name_depth", "alt_depth"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_depth/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_depth/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, SEGMENT_Alternative_StreamName) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::SEGMENTED); + nh->setParam("cam_config/test_cam/name_segment", "alt_seg"); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + + bool img = false, found_info = false; + + ros::master::V_TopicInfo master_topics; + ros::master::getTopics(master_topics); + + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_seg/image_raw") { + img = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/alt_seg/camera_info") { + found_info = true; + } + if (img && found_info) + break; + } + EXPECT_TRUE(img && found_info); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_Published_Correctly) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("unpause", false); + nh->setParam("cam_config/test_cam/frequency", 30); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + std::vector rgb_images; + std::vector rgb_infos; + + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe( + "cameras/test_cam/rgb/image_raw", 1, + [&rgb_images](const sensor_msgs::Image::ConstPtr &msg) { rgb_images.emplace_back(*msg); }); + ros::Subscriber info_sub = nh->subscribe( + "cameras/test_cam/rgb/camera_info", 1, + [&rgb_infos](const sensor_msgs::CameraInfo::ConstPtr &msg) { rgb_infos.emplace_back(*msg); }); + + env_ptr->startWithXML(xml_path); + env_ptr->step(1); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->rgb_pub_.getNumSubscribers(), 1); + + // Wait for image to be published with 1s timeout + float seconds = 0.f; + while ((rgb_images.empty() || rgb_infos.empty()) && seconds < 1.f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + EXPECT_LT(seconds, 1.f) << "RGB image not published within 1s"; + + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30); + + ASSERT_EQ(rgb_images.size(), 1); + ASSERT_EQ(rgb_infos.size(), 1); + + ros::Time t1 = ros::Time::now(); + EXPECT_EQ(rgb_images[0].header.stamp, t1); + EXPECT_STREQ(rgb_images[0].header.frame_id.c_str(), "test_cam_optical_frame"); + EXPECT_EQ(rgb_images[0].width, 7); + EXPECT_EQ(rgb_images[0].height, 4); + EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); + + EXPECT_EQ(rgb_infos[0].header.stamp, t1); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, Cam_Timing_Correct) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("unpause", false); + nh->setParam("cam_config/test_cam/frequency", 30); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + std::vector rgb_images; + std::vector rgb_infos; + + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe( + "cameras/test_cam/rgb/image_raw", 1, + [&rgb_images](const sensor_msgs::Image::ConstPtr &msg) { rgb_images.emplace_back(*msg); }); + ros::Subscriber info_sub = nh->subscribe( + "cameras/test_cam/rgb/camera_info", 1, + [&rgb_infos](const sensor_msgs::CameraInfo::ConstPtr &msg) { rgb_infos.emplace_back(*msg); }); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + env_ptr->step(1); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + + // Check default camera settings + EXPECT_EQ(offscreen->cams[0]->cam_id_, 0); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30); + + // Wait for image to be published with 400ms timeout + float seconds = 0.f; + while ((rgb_images.empty() || rgb_infos.empty()) && seconds < .4f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; + + ASSERT_EQ(rgb_infos.size(), 1); + ASSERT_EQ(rgb_images.size(), 1); + + ros::Time t1 = ros::Time::now(); + // Step the simulation to as to trigger the camera rendering + mjModel *m = env_ptr->getModelPtr(); + int n_steps = std::ceil((1.0 / 30.0) / env_ptr->getModelPtr()->opt.timestep); + + env_ptr->step(n_steps - 1); + // wait a little + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + + // should not have received a new image yet + ASSERT_EQ(rgb_infos.size(), 1); + ASSERT_EQ(rgb_images.size(), 1); + + env_ptr->step(1); + // Wait for image to be published with 400ms timeout + seconds = 0.f; + while ((rgb_images.size() < 2 || rgb_infos.size() < 2) && seconds < .4f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + // should now have received new image + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; + + ASSERT_EQ(rgb_infos.size(), 2); + ASSERT_EQ(rgb_images.size(), 2); + ros::Time t2 = ros::Time::now(); + + ASSERT_EQ(rgb_images[0].header.stamp, t1); + ASSERT_EQ(rgb_images[1].header.stamp, t2); + + ASSERT_EQ(rgb_infos[0].header.stamp, t1); + ASSERT_EQ(rgb_infos[1].header.stamp, t2); + + // int n_steps = std::ceil((1.0 / 30.0) / env_ptr->getModelPtr()->opt.timestep); + ros::Time t3 = t2 + ros::Duration((std::ceil((1.0 / 30.0) / m->opt.timestep)) * m->opt.timestep); + // ros::Time t3 = t2 + (t2 - t1); + // Step over next image trigger but before trigger after that + env_ptr->step(2 * n_steps - 1); + + ASSERT_EQ(rgb_infos.size(), 3); + ASSERT_EQ(rgb_images.size(), 3); + + // Check that the timestamps are as expected + EXPECT_EQ(rgb_images[2].header.stamp, rgb_infos[2].header.stamp); + EXPECT_EQ(rgb_images[2].header.stamp, t3); + EXPECT_EQ(rgb_infos[2].header.stamp, t3); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_Image_Dtype) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("unpause", false); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + std::vector rgb_images; + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe( + "cameras/test_cam/rgb/image_raw", 1, + [&rgb_images](const sensor_msgs::Image::ConstPtr &msg) { rgb_images.emplace_back(*msg); }); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + env_ptr->step(1); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->width_, 7); + EXPECT_EQ(offscreen->cams[0]->height_, 4); + + // Wait for image to be published with 400ms timeout + float seconds = 0.f; + while (rgb_images.empty() && seconds < .4f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; + + ASSERT_EQ(rgb_images.size(), 1); + EXPECT_EQ(rgb_images[0].data.size(), 7 * 4 * 3); + EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("unpause", false); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::DEPTH); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + std::vector depth_images; + + // Subscribe to topic + ros::Subscriber depth_sub = nh->subscribe( + "cameras/test_cam/depth/image_raw", 1, + [&depth_images](const sensor_msgs::Image::ConstPtr &msg) { depth_images.emplace_back(*msg); }); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + env_ptr->step(1); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::DEPTH); + EXPECT_EQ(offscreen->cams[0]->width_, 7); + EXPECT_EQ(offscreen->cams[0]->height_, 4); + + // Wait for image to be published with 200ms timeout + float seconds = 0.f; + while (depth_images.empty() && seconds < .2f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + EXPECT_LT(seconds, .2f) << "Depth image not published within 200ms"; + + ASSERT_EQ(depth_images.size(), 1); + EXPECT_EQ(depth_images[0].width, 7); + EXPECT_EQ(depth_images[0].height, 4); + EXPECT_EQ(depth_images[0].encoding, sensor_msgs::image_encodings::TYPE_32FC1); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) +{ + nh->setParam("no_render", false); + nh->setParam("headless", true); + nh->setParam("unpause", false); + nh->setParam("cam_config/test_cam/stream_type", rendering::streamType::SEGMENTED); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + std::vector seg_images; + + // Subscribe to topic + ros::Subscriber seg_sub = nh->subscribe( + "cameras/test_cam/segmented/image_raw", 1, + [&seg_images](const sensor_msgs::Image::ConstPtr &msg) { seg_images.emplace_back(*msg); }); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + env_ptr->step(1); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + ASSERT_EQ(offscreen->cams.size(), 1); + EXPECT_STREQ(offscreen->cams[0]->cam_name_.c_str(), "test_cam"); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::SEGMENTED); + EXPECT_EQ(offscreen->cams[0]->width_, 7); + EXPECT_EQ(offscreen->cams[0]->height_, 4); + + // Wait for image to be published with 200ms timeout + float seconds = 0.f; + while (seg_images.empty() && seconds < .2f) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001f; + } + EXPECT_LT(seconds, .2f) << "Segmentation image not published within 200ms"; + + ASSERT_EQ(seg_images.size(), 1); + EXPECT_EQ(seg_images[0].data.size(), 7 * 4 * 3); + EXPECT_EQ(seg_images[0].encoding, sensor_msgs::image_encodings::RGB8); + + env_ptr->shutdown(); +} + +#endif // RENDER_BACKEND == USE_GLFW || RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA // i.e. any render + // backend available + +#if RENDER_BACKEND == USE_NONE // i.e. no render backend available +TEST_F(BaseEnvFixture, No_Render_Backend_Headless_Warn) +{ + nh->setParam("headless", true); + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + env_ptr->startWithXML(xml_path); + + while (env_ptr->getOperationalStatus() != 0) { // wait for model to be loaded + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + } + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_FALSE(env_ptr->settings_.render_offscreen); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + EXPECT_TRUE(offscreen->cams.empty()); + + env_ptr->shutdown(); +} +#endif // RENDER_BACKEND == USE_NONE // i.e. no render backend available diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index 6d4de568..c334d298 100644 --- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp +++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp @@ -41,6 +41,7 @@ #include "mujoco_env_fixture.h" #include "test_plugin/test_plugin.h" +#include #include #include #include @@ -73,7 +74,8 @@ class LoadedPluginFixture : public ::testing::Test { nh = std::make_unique("~"); nh->setParam("unpause", false); - nh->setParam("no_x", true); + nh->setParam("no_render", true); + nh->setParam("headless", true); nh->setParam("use_sim_time", true); env_ptr = new MujocoEnvTestWrapper(); @@ -98,6 +100,8 @@ class LoadedPluginFixture : public ::testing::Test void TearDown() override { + // cleanup all parameters + ros::param::del(nh->getNamespace()); test_plugin = nullptr; env_ptr->shutdown(); delete env_ptr; @@ -124,10 +128,88 @@ TEST_F(LoadedPluginFixture, PassiveCallback) EXPECT_TRUE(test_plugin->ran_passive_cb.load()); } -// TODO: Involves offscreen rendering, can we do this in tests? -// TEST_F(LoadedPluginFixture, RenderCallback) { -// EXPECT_TRUE(test_plugin->ran_render_cb.load()); -// } +#if RENDER_BACKEND == GLFW || RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA +TEST_F(BaseEnvFixture, RenderCallback) +{ + nh->setParam("no_render", false); + nh->setParam("unpause", false); + nh->setParam("headless", true); + nh->setParam("cam_config/test_cam/width", 7); + nh->setParam("cam_config/test_cam/height", 4); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // NOP subscriber to trigger render callback + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, + [&](const sensor_msgs::Image::ConstPtr & /*msg*/) {}); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->step()); + + OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); + EXPECT_TRUE(offscreen->cams.size() == 1); + + TestPlugin *test_plugin = nullptr; + auto &plugins = env_ptr->getPlugins(); + for (const auto &p : plugins) { + test_plugin = dynamic_cast(p.get()); + if (test_plugin != nullptr) { + break; + } + } + + // wait for render callback to be called + float seconds = 0; + while (!test_plugin->ran_render_cb.load() && seconds < 1.) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, 1) << "Render callback was not called within 1 second!"; + + env_ptr->shutdown(); +} +#endif + +#if RENDER_BACKEND == NONE +TEST_F(BaseEnvFixture, RenderCallback_NoRender) +{ + nh->setParam("no_render", false); + nh->setParam("unpause", false); + nh->setParam("headless", true); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // NOP subscriber to trigger render callback + ros::Subscriber rgb_sub = nh->subscribe( + "cameras/test_cam/rgb/image_raw", 1, + [&](const sensor_msgs::Image::ConstPtr & /*msg*/) { ROS_ERROR("Got image!"); }); + + env_ptr->startWithXML(xml_path); + EXPECT_TRUE(env_ptr->step(5)); + + TestPlugin *test_plugin = nullptr; + auto &plugins = env_ptr->getPlugins(); + for (const auto &p : plugins) { + test_plugin = dynamic_cast(p.get()); + if (test_plugin != nullptr) { + break; + } + } + + // wait for render callback to be called + float seconds = 0; + while (!test_plugin->ran_render_cb.load() && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_FALSE(test_plugin->ran_render_cb.load()) << "Render callback was called!"; + + env_ptr->shutdown(); +} +#endif TEST_F(LoadedPluginFixture, LastCallback) { @@ -145,22 +227,23 @@ TEST_F(LoadedPluginFixture, OnGeomChangedCallback) TEST_F(BaseEnvFixture, LoadPlugin) { + nh->setParam("no_render", true); nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env loading ran into 2 seconds timeout!"; - EXPECT_EQ(env.getPlugins().size(), 1) << "Env should have 1 plugin registered!"; - EXPECT_EQ(env.getNumCBReadyPlugins(), 1) << "Env should have 1 plugin loaded!"; + EXPECT_EQ(env_ptr->getPlugins().size(), 1) << "Env should have 1 plugin registered!"; + EXPECT_EQ(env_ptr->getNumCBReadyPlugins(), 1) << "Env should have 1 plugin loaded!"; - env.shutdown(); + env_ptr->shutdown(); } TEST_F(LoadedPluginFixture, ResetPlugin) @@ -200,24 +283,24 @@ TEST_F(BaseEnvFixture, FailedLoad) nh->setParam("should_fail", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env loading ran into 2 seconds timeout!"; - EXPECT_EQ(env.getPlugins().size(), 1) << "Env should have 1 plugin registered!"; - EXPECT_EQ(env.getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; + EXPECT_EQ(env_ptr->getPlugins().size(), 1) << "Env should have 1 plugin registered!"; + EXPECT_EQ(env_ptr->getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; { TestPlugin *test_plugin = nullptr; - auto &plugins = env.getPlugins(); + auto &plugins = env_ptr->getPlugins(); for (const auto &p : plugins) { test_plugin = dynamic_cast(p.get()); if (test_plugin != nullptr) { @@ -234,8 +317,7 @@ TEST_F(BaseEnvFixture, FailedLoad) EXPECT_FALSE(test_plugin->ran_on_geom_changed_cb.load()); } - env.shutdown(); - nh->setParam("should_fail", false); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, FailedLoadRecoverReload) @@ -243,24 +325,24 @@ TEST_F(BaseEnvFixture, FailedLoadRecoverReload) nh->setParam("should_fail", true); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env loading ran into 2 seconds timeout!"; - EXPECT_EQ(env.getPlugins().size(), 1) << "Env should have 1 plugin registered!"; - EXPECT_EQ(env.getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; + EXPECT_EQ(env_ptr->getPlugins().size(), 1) << "Env should have 1 plugin registered!"; + EXPECT_EQ(env_ptr->getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; { TestPlugin *test_plugin = nullptr; - auto &plugins = env.getPlugins(); + auto &plugins = env_ptr->getPlugins(); for (const auto &p : plugins) { test_plugin = dynamic_cast(p.get()); if (test_plugin != nullptr) { @@ -270,18 +352,18 @@ TEST_F(BaseEnvFixture, FailedLoadRecoverReload) nh->setParam("should_fail", false); - env.settings_.load_request = 2; - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { + env_ptr->settings_.load_request = 2; + float seconds = 0; + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env reset ran into 2 seconds timeout!"; - EXPECT_EQ(env.getPlugins().size(), 1) << "Env should have 1 plugin registered!"; - EXPECT_EQ(env.getNumCBReadyPlugins(), 1) << "Env should have 1 plugin loaded!"; + EXPECT_EQ(env_ptr->getPlugins().size(), 1) << "Env should have 1 plugin registered!"; + EXPECT_EQ(env_ptr->getNumCBReadyPlugins(), 1) << "Env should have 1 plugin loaded!"; } - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, FailedLoadReset) @@ -290,24 +372,24 @@ TEST_F(BaseEnvFixture, FailedLoadReset) nh->setParam("unpause", false); std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/empty_world.xml"; - MujocoEnvTestWrapper env; + env_ptr = std::make_unique(""); - env.startWithXML(xml_path); + env_ptr->startWithXML(xml_path); float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { + while (env_ptr->getOperationalStatus() != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env loading ran into 2 seconds timeout!"; - EXPECT_EQ(env.getPlugins().size(), 1) << "Env should have 1 plugin registered!"; - EXPECT_EQ(env.getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; + EXPECT_EQ(env_ptr->getPlugins().size(), 1) << "Env should have 1 plugin registered!"; + EXPECT_EQ(env_ptr->getNumCBReadyPlugins(), 0) << "Env should have 0 plugins loaded!"; { TestPlugin *test_plugin = nullptr; - auto &plugins = env.getPlugins(); + auto &plugins = env_ptr->getPlugins(); for (const auto &p : plugins) { test_plugin = dynamic_cast(p.get()); if (test_plugin != nullptr) { @@ -315,20 +397,19 @@ TEST_F(BaseEnvFixture, FailedLoadReset) } } - env.settings_.reset_request = 1; - float seconds = 0; - while (env.settings_.reset_request != 0 && seconds < 2) { + env_ptr->settings_.reset_request = 1; + float seconds = 0; + while (env_ptr->settings_.reset_request != 0 && seconds < 2) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } EXPECT_LT(seconds, 2) << "Env reset ran into 2 seconds timeout!"; - env.step(10); + env_ptr->step(10); EXPECT_FALSE(test_plugin->ran_reset.load()) << "Dummy plugin should not have beeon reset!"; } - env.shutdown(); - nh->setParam("should_fail", false); + env_ptr->shutdown(); } TEST_F(LoadedPluginFixture, PluginStats_InitialPaused) diff --git a/mujoco_ros/test/ros_interface_test.cpp b/mujoco_ros/test/ros_interface_test.cpp index c951e8a7..fd8ef04a 100644 --- a/mujoco_ros/test/ros_interface_test.cpp +++ b/mujoco_ros/test/ros_interface_test.cpp @@ -409,15 +409,15 @@ TEST_F(BaseEnvFixture, CustomInitialJointStates) nh->setParam("initial_joint_positions/joint_map", pos_map); nh->setParam("initial_joint_velocities/joint_map", vel_map); - MujocoEnvTestWrapper env; - env.startWithXML(xml_path); + env_ptr = std::make_unique(""); + env_ptr->startWithXML(xml_path); - while (env.getOperationalStatus() > 0) { // wait for reset to be done + while (env_ptr->getOperationalStatus() > 0) { // wait for reset to be done std::this_thread::sleep_for(std::chrono::milliseconds(1)); } - mjData *d = env.getDataPtr(); - mjModel *m = env.getModelPtr(); + mjData *d = env_ptr->getDataPtr(); + mjModel *m = env_ptr->getModelPtr(); int id_balljoint, id1, id2, id_free; @@ -442,11 +442,7 @@ TEST_F(BaseEnvFixture, CustomInitialJointStates) compare_qvel(d, m->jnt_dofadr[id2], "joint2", { 1.05 }); compare_qvel(d, m->jnt_dofadr[id_free], "ball_freejoint", { 1.0, 2.0, 3.0, 10.0, 20.0, 30.0 }); - env.shutdown(); - - nh->deleteParam("initial_joint_positions/joint_map"); - nh->deleteParam("initial_joint_velocities/joint_map"); - nh->setParam("unpause", true); + env_ptr->shutdown(); } TEST_F(PendulumEnvFixture, CustomInitialJointStatesOnReset) diff --git a/mujoco_ros/test/test_util.h b/mujoco_ros/test/test_util.h index 5a183676..f337c0ba 100644 --- a/mujoco_ros/test/test_util.h +++ b/mujoco_ros/test/test_util.h @@ -39,11 +39,11 @@ #include using namespace mujoco_ros; -void load_queued_model(MujocoEnv &env) +void load_queued_model(MujocoEnv *env) { - env.settings_.load_request = 2; - float seconds = 0; - while (env.getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout + env->settings_.load_request = 2; + float seconds = 0; + while (env->getOperationalStatus() != 0 && seconds < 2) { // wait for model to be loaded or timeout std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001; } diff --git a/mujoco_ros_laser/test/mujoco_env_wrapper.h b/mujoco_ros_laser/test/mujoco_env_wrapper.h index caba900c..5b9e417b 100644 --- a/mujoco_ros_laser/test/mujoco_env_wrapper.h +++ b/mujoco_ros_laser/test/mujoco_env_wrapper.h @@ -77,7 +77,7 @@ class MujocoEnvTestWrapper : public MujocoEnv const std::string &getHandleNamespace() { return nh_->getNamespace(); } - void startWithXML(const std::string &xml_path) + void startWithXML(const std::string &xml_path, bool wait = false) { mju::strcpy_arr(queued_filename_, xml_path.c_str()); settings_.load_request = 2; diff --git a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp index 111c0d60..9b339275 100644 --- a/mujoco_ros_sensors/test/mujoco_sensors_test.cpp +++ b/mujoco_ros_sensors/test/mujoco_sensors_test.cpp @@ -36,6 +36,7 @@ #include +#include #include #include #include