From 5f42d8c5c7c1e9edd870a7d343c5d55c6fc84570 Mon Sep 17 00:00:00 2001 From: David Leins Date: Wed, 20 Nov 2024 10:38:52 +0100 Subject: [PATCH 01/17] test: makes BaseEnvFixutre cleanup params on teardown --- mujoco_ros/test/mujoco_env_fixture.h | 6 +++++- mujoco_ros/test/mujoco_env_test.cpp | 13 ------------- mujoco_ros/test/mujoco_ros_plugin_test.cpp | 2 -- mujoco_ros/test/ros_interface_test.cpp | 4 ---- 4 files changed, 5 insertions(+), 20 deletions(-) diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index 3934484e..e4584f25 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -103,7 +103,11 @@ class BaseEnvFixture : public ::testing::Test nh->setParam("use_sim_time", true); } - void TearDown() override {} + void TearDown() override + { + // clean up all parameters + ros::param::del(nh->getNamespace()); + } }; class PendulumEnvFixture : public ::testing::Test diff --git a/mujoco_ros/test/mujoco_env_test.cpp b/mujoco_ros/test/mujoco_env_test.cpp index b7307075..91118bb5 100644 --- a/mujoco_ros/test/mujoco_env_test.cpp +++ b/mujoco_ros/test/mujoco_env_test.cpp @@ -61,7 +61,6 @@ 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); } TEST_F(BaseEnvFixture, RunEvalMode) @@ -81,7 +80,6 @@ TEST_F(BaseEnvFixture, RunEvalMode) EXPECT_FALSE(env.settings_.exit_request) << "Exit request is set before shutdown!"; env.shutdown(); - nh->setParam("eval_mode", false); } TEST_F(BaseEnvFixture, EvalPauseWithHash) @@ -104,7 +102,6 @@ TEST_F(BaseEnvFixture, EvalPauseWithHash) EXPECT_FALSE(env.settings_.run) << "Model should not be running!"; env.shutdown(); - nh->setParam("eval_mode", false); } TEST_F(BaseEnvFixture, EvalPauseWithoutHashForbidden) @@ -127,7 +124,6 @@ TEST_F(BaseEnvFixture, EvalPauseWithoutHashForbidden) EXPECT_TRUE(env.settings_.run) << "Model should still be running!"; env.shutdown(); - nh->setParam("eval_mode", false); } TEST_F(BaseEnvFixture, EvalUnpauseWithHash) @@ -149,7 +145,6 @@ TEST_F(BaseEnvFixture, EvalUnpauseWithHash) EXPECT_TRUE(env.settings_.run) << "Model should be running!"; env.shutdown(); - nh->setParam("eval_mode", false); } TEST_F(BaseEnvFixture, StepBeforeLoad) @@ -200,7 +195,6 @@ TEST_F(BaseEnvFixture, StepSingleWhilePaused) EXPECT_DOUBLE_EQ(env.getDataPtr()->time, env.getModelPtr()->opt.timestep); env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, StepMultiWhilePaused) @@ -221,7 +215,6 @@ TEST_F(BaseEnvFixture, StepMultiWhilePaused) EXPECT_NEAR(env.getDataPtr()->time, 100 * env.getModelPtr()->opt.timestep, 1e-6); env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, StepUnblocked) @@ -249,7 +242,6 @@ TEST_F(BaseEnvFixture, StepUnblocked) EXPECT_LT(seconds, 2) << "Time should have passed but ran into 2 seconds timeout!"; env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, StepNegativeFail) @@ -271,7 +263,6 @@ TEST_F(BaseEnvFixture, StepNegativeFail) EXPECT_DOUBLE_EQ(env.getDataPtr()->time, 0.0); env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, Shutdown) @@ -384,7 +375,6 @@ TEST_F(BaseEnvFixture, PauseUnpause) EXPECT_LT(seconds, 2) << "Time should have been moving forward in unpaused state, ran into 2 seconds timeout!"; env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, StepsTerminate) @@ -420,7 +410,6 @@ TEST_F(BaseEnvFixture, StepsTerminate) EXPECT_NEAR(env.getDataPtr()->time, env.getModelPtr()->opt.timestep * 100, env.getModelPtr()->opt.timestep * 0.1) << "Time should have stopped after 100 steps"; - nh->deleteParam("num_steps"); env.shutdown(); } @@ -477,7 +466,6 @@ TEST_F(BaseEnvFixture, ManualSteps) << "Time should have been increased by 100*timestep!"; env.shutdown(); - nh->setParam("unpause", true); } TEST_F(BaseEnvFixture, Reset) @@ -572,7 +560,6 @@ TEST_F(BaseEnvFixture, Reload) } env.shutdown(); - nh->setParam("unpause", false); } TEST_F(BaseEnvFixture, InitModelFromQueuedBuffer) diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index 6d4de568..f56fc338 100644 --- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp +++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp @@ -235,7 +235,6 @@ TEST_F(BaseEnvFixture, FailedLoad) } env.shutdown(); - nh->setParam("should_fail", false); } TEST_F(BaseEnvFixture, FailedLoadRecoverReload) @@ -328,7 +327,6 @@ TEST_F(BaseEnvFixture, FailedLoadReset) } env.shutdown(); - nh->setParam("should_fail", false); } 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..982e7e42 100644 --- a/mujoco_ros/test/ros_interface_test.cpp +++ b/mujoco_ros/test/ros_interface_test.cpp @@ -443,10 +443,6 @@ TEST_F(BaseEnvFixture, CustomInitialJointStates) 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); } TEST_F(PendulumEnvFixture, CustomInitialJointStatesOnReset) From dd9b389c95c17d447c5294977263c9e010a29beb Mon Sep 17 00:00:00 2001 From: David Leins Date: Wed, 20 Nov 2024 11:21:35 +0100 Subject: [PATCH 02/17] chg: headless no longer implies no offscreen --- mujoco_ros/src/main.cpp | 9 +-------- mujoco_ros/src/mujoco_env.cpp | 25 ++++++++++++++----------- 2 files changed, 15 insertions(+), 19 deletions(-) diff --git a/mujoco_ros/src/main.cpp b/mujoco_ros/src/main.cpp index 547c9a66..34c16fe0 100644 --- a/mujoco_ros/src/main.cpp +++ b/mujoco_ros/src/main.cpp @@ -138,13 +138,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,7 +147,7 @@ int main(int argc, char **argv) env->startPhysicsLoop(); env->startEventLoop(); - if (!no_x) { + if (!env->settings_.headless) { // mjvCamera cam; // mjvOption opt; // mjv_defaultCamera(&cam); diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index e63494b7..6419c20f 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -75,10 +75,19 @@ 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_x; nh_ = std::make_unique("~"); ROS_DEBUG_STREAM("New MujocoEnv created"); + nh_->param("no_x", no_x, false); + + if (no_x) { + ROS_INFO("no_x is set to true. 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 @@ -104,19 +113,13 @@ 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) { gui_adapter_ = new mujoco_ros::GlfwAdapter(); } @@ -212,7 +215,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(); From 38653662af01fa24e0191148b8450b177e9313fa Mon Sep 17 00:00:00 2001 From: David Leins Date: Wed, 20 Nov 2024 11:22:14 +0100 Subject: [PATCH 03/17] fix: freeing offscreen context in headless mode previously, in case a render request is still active, freeing the context would case a segfault. Instead now the render mutex is locked, the pending request is canceled and the context freed. --- CHANGELOG.md | 1 + mujoco_ros/src/offscreen_rendering.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index a43364c8..b6b61741 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,7 @@ 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. ### Changed * Moved `mujoco_ros::Viewer::Clock` definition to `mujoco_ros::Clock` (into common_types.h). diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 9bd204ac..554d6a11 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -45,9 +45,10 @@ namespace mujoco_ros { OffscreenRenderContext::~OffscreenRenderContext() { 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"); + std::unique_lock lock(render_mutex); + request_pending.store(false); + mjr_defaultContext(&con); mjr_freeContext(&con); } } From 2bfb6b998d78b777a340aa7137af55d38f883c11 Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 21 Nov 2024 16:46:25 +0100 Subject: [PATCH 04/17] build: adds option to build w/ GLFW, EGL or OSMesa Default build tries the following: If GLFW is found, it will be used for GUI and offscreen rendering. Otherwise GUI functionality is not provided. As fallback for offscreen rendering first GLE is searched, otherwise osmesa. In case neither can be found, offscreen rendering is disabled, too. --- CHANGELOG.md | 1 + mujoco_ros/CMakeLists.txt | 34 ++++- mujoco_ros/cmake/FindOSMesa.cmake | 60 ++++++++ mujoco_ros/cmake/Findmujoco.cmake | 2 +- mujoco_ros/include/mujoco_ros/common_types.h | 2 - mujoco_ros/include/mujoco_ros/mujoco_env.h | 18 +++ mujoco_ros/launch/launch_server.launch | 7 +- mujoco_ros/src/CMakeLists.txt | 64 +++++---- mujoco_ros/src/main.cpp | 25 ++-- mujoco_ros/src/mujoco_env.cpp | 49 ++++++- mujoco_ros/src/offscreen_camera.cpp | 2 + mujoco_ros/src/offscreen_rendering.cpp | 136 ++++++++++++++++++- mujoco_ros/test/mujoco_env_fixture.h | 6 +- 13 files changed, 354 insertions(+), 52 deletions(-) create mode 100644 mujoco_ros/cmake/FindOSMesa.cmake diff --git a/CHANGELOG.md b/CHANGELOG.md index b6b61741..641174d3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,6 +27,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c * 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. Contributors: @DavidPL1 diff --git a/mujoco_ros/CMakeLists.txt b/mujoco_ros/CMakeLists.txt index d809cfbc..5bff9680 100644 --- a/mujoco_ros/CMakeLists.txt +++ b/mujoco_ros/CMakeLists.txt @@ -21,6 +21,10 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON) list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") include(ProjectOption) +option(NO_GLFW "Disable GLFW3 support and try finding OpenGL EGL or OSMESA for offscreen rendering" OFF) +option(USE_EGL "Build with EGL enabled offscreen rendering. Entails `NO_GLFW`" OFF) +option(USE_OSMESA "Build with OSMESA enabled offscreen rendering. Entails `NO_GLFW`" OFF) + # Find catkin macros and libraries # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) # is used, also find other catkin packages @@ -60,7 +64,35 @@ configure_project_option( # Find MuJoCo find_package(mujoco 3.2.0 REQUIRED) -find_library(GLFW libglfw.so.3) +find_library(GLFW libglfw.so.3) # Find GLFW3 for GUI + + +if (NO_GLFW OR USE_EGL OR USE_OSMESA 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 (USE_OSMESA OR ${OpenGL_EGL_FOUND} STREQUAL "FALSE") + message(WARNING "EGL not found or disabled. Falling back to OSMESA.") + + find_package(OSMesa) + + if (!OSMesa_FOUND) + message(WARNING "EGL disabled or not found and OSMesa could not be found. Offscreen rendering will not be available!") + 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() + # ############################################### # # Declare ROS dynamic reconfigure parameters ## 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/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..2b87b835 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -90,8 +90,12 @@ #include +#if defined(USE_GLFW) #include #include +#elif defined(USE_OSMESA) +#include +#endif namespace mujoco_ros { @@ -116,7 +120,15 @@ struct OffscreenRenderContext mjvCamera cam; std::unique_ptr rgb; std::unique_ptr depth; +#ifdef USE_GLFW std::shared_ptr window; +#elif defined(USE_OSMESA) + struct + { + OSMesaContext ctx; + unsigned char buffer[10000000]; // TODO: size necessary or resize later? + } osmesa; +#endif mjrContext con = {}; mjvScene scn = {}; @@ -268,7 +280,13 @@ class MujocoEnv bool togglePaused(bool paused, const std::string &admin_hash = std::string()); +#if defined(USE_GLFW) GlfwAdapter *gui_adapter_ = nullptr; +#endif + +#if defined(USE_EGL) || defined(USE_OSMESA) + bool InitGL(); +#endif void runRenderCbs(mjvScene *scene); bool step(int num_steps = 1, bool blocking = true); 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 34c16fe0..f565f072 100644 --- a/mujoco_ros/src/main.cpp +++ b/mujoco_ros/src/main.cpp @@ -34,7 +34,6 @@ /* Authors: David P. Leins */ -#include #include #include @@ -45,6 +44,10 @@ #include #include +#if defined(USE_GLFW) +#include +#endif + namespace { std::unique_ptr env; @@ -147,19 +150,19 @@ int main(int argc, char **argv) env->startPhysicsLoop(); env->startEventLoop(); +#ifdef USE_GLFW if (!env->settings_.headless) { - // mjvCamera cam; - // mjvOption opt; - // mjv_defaultCamera(&cam); - // mjv_defaultOption(&opt); 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 6419c20f..c1c10c9c 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -43,12 +43,23 @@ #include #include +#if defined(USE_GLFW) +static std::string render_backend = "GLFW"; +#elif defined(USE_OSMESA) +static std::string render_backend = "OSMESA"; +#elif defined(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; +#ifdef USE_GLFW namespace { int MaybeGlfwInit() { @@ -62,6 +73,7 @@ int MaybeGlfwInit() return is_initialized; } } // namespace +#endif MujocoEnv *MujocoEnv::instance = nullptr; @@ -75,15 +87,19 @@ 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_x; + bool no_render; nh_ = std::make_unique("~"); ROS_DEBUG_STREAM("New MujocoEnv created"); - nh_->param("no_x", no_x, false); + 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_x) { - ROS_INFO("no_x is set to true. Disabling rendering and setting headless to true"); + 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); } @@ -93,6 +109,7 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) 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()); @@ -120,7 +137,11 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) nh_->param("headless", settings_.headless, true); if (!settings_.headless) { +#if defined(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) { @@ -137,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 defined(USE_GLFW) + can_render = MaybeGlfwInit(); + ROS_ERROR_COND(!can_render, "Failed to initialize GLFW. Cannot render offscreen!"); +#elif !defined(USE_EGL) && !defined(USE_OSMESA) + 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); diff --git a/mujoco_ros/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index 47b34bc9..3f03ea7a 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -192,7 +192,9 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext } else if (depth) { mjr_readPixels(nullptr, offscreen->depth.get(), viewport, &offscreen->con); } +#ifdef USE_GLFW glfwSwapBuffers(offscreen->window.get()); +#endif if (rgb) { // Publish RGB image diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 554d6a11..6abb1147 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -40,17 +40,48 @@ #include +#ifdef USE_EGL +#include +#endif + namespace mujoco_ros { OffscreenRenderContext::~OffscreenRenderContext() { +#if defined(USE_GLFW) if (window != nullptr) { - 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 defined(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 defined(USE_OSMESA) + ROS_DEBUG("Freeing OSMESA offscreen context"); + mjr_defaultContext(&con); + mjr_freeContext(&con); + OSMesaDestroyContext(osmesa.ctx); +#endif } void MujocoEnv::initializeRenderResources() @@ -122,9 +153,11 @@ void MujocoEnv::initializeRenderResources() ROS_DEBUG_NAMED("offscreen_rendering", "Initializing offscreen rendering utils"); +#ifdef 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"); @@ -132,9 +165,93 @@ void MujocoEnv::initializeRenderResources() mjr_setBuffer(mjFB_OFFSCREEN, &offscreen_.con); } +#if defined(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 defined(USE_OSMESA) +bool MujocoEnv::InitGL() +{ + ROS_DEBUG("Initializing OSMesa..."); + // Initialize OSMesa + offscreen_.osmesa.ctx = OSMesaCreateContextExt(GL_RGBA, 24, 8, 8, 0); + 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"); + return true; +} // InitGL +#endif + void MujocoEnv::offscreenRenderLoop() { - is_rendering_running_ = 1; +#if defined(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), @@ -150,7 +267,22 @@ void MujocoEnv::offscreenRenderLoop() Glfw().glfwMakeContextCurrent(offscreen_.window.get()); Glfw().glfwSwapInterval(0); +#elif defined(USE_EGL) + if (!InitGL()) { + ROS_ERROR("Failed to initialize EGL. Cannot run offscreen rendering"); + return; + } +#elif defined(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 diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index e4584f25..c5118927 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -99,7 +99,7 @@ class BaseEnvFixture : public ::testing::Test { nh = std::make_unique("~"); nh->setParam("unpause", true); - nh->setParam("no_x", true); + nh->setParam("no_render", true); nh->setParam("use_sim_time", true); } @@ -120,7 +120,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); @@ -162,7 +162,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); From 50cacdb069653809dca59484d0befa9efef71597 Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 25 Nov 2024 20:22:30 +0100 Subject: [PATCH 05/17] fix: offscreen render pipeline improvements * fixes edge case where model was missing fields because of a missing forward computation which caused issues when applying the model to the offscreen scene. * fixes possible deadlock during shutdown when waiting to join the render loop. --- mujoco_ros/include/mujoco_ros/mujoco_env.h | 2 +- mujoco_ros/src/mujoco_env.cpp | 3 +++ mujoco_ros/src/offscreen_rendering.cpp | 7 +++++-- mujoco_ros/src/physics.cpp | 1 - 4 files changed, 9 insertions(+), 4 deletions(-) diff --git a/mujoco_ros/include/mujoco_ros/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 2b87b835..7b0c6fde 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -341,9 +341,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/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index c1c10c9c..dc418a44 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -607,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_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 6abb1147..1e2d1743 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -305,8 +305,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()) { @@ -317,6 +319,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(); From fe584eacb5d48b67601e5ecba4e84f18c8ef7a03 Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 25 Nov 2024 20:31:47 +0100 Subject: [PATCH 06/17] chg: updates BaseEnvFixture and tests using it --- mujoco_ros/test/mujoco_env_fixture.h | 16 +- mujoco_ros/test/mujoco_env_test.cpp | 445 ++++++++------------- mujoco_ros/test/mujoco_ros_plugin_test.cpp | 72 ++-- mujoco_ros/test/ros_interface_test.cpp | 12 +- mujoco_ros/test/test_util.h | 8 +- mujoco_ros_laser/test/mujoco_env_wrapper.h | 2 +- 6 files changed, 234 insertions(+), 321 deletions(-) diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index c5118927..c0bcb07f 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -81,12 +81,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,6 +104,7 @@ class BaseEnvFixture : public ::testing::Test { protected: std::unique_ptr nh; + std::unique_ptr env_ptr; void SetUp() override { @@ -105,6 +116,9 @@ class BaseEnvFixture : public ::testing::Test void TearDown() override { + if (env_ptr != nullptr) { + env_ptr->shutdown(); + } // clean up all parameters ros::param::del(nh->getNamespace()); } diff --git a/mujoco_ros/test/mujoco_env_test.cpp b/mujoco_ros/test/mujoco_env_test.cpp index 91118bb5..1140d373 100644 --- a/mujoco_ros/test/mujoco_env_test.cpp +++ b/mujoco_ros/test/mujoco_env_test.cpp @@ -67,335 +67,250 @@ 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(); + 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_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, "some_hash"); - EXPECT_FALSE(env.settings_.run) << "Model should not be running!"; + env_ptr->togglePaused(true, "some_hash"); + EXPECT_FALSE(env_ptr->settings_.run) << "Model should not be running!"; - env.shutdown(); -} - -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.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); - EXPECT_TRUE(env.settings_.run) << "Model should still be running!"; - - env.shutdown(); + 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(); + 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(); + 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(); + 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(); + 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_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.shutdown(); + 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); + 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_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(); + 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; @@ -408,112 +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"; - 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); + env_ptr->startWithXML(xml_path); - while (env.getOperationalStatus() != 0) { // wait for model to be loaded - std::this_thread::sleep_for(std::chrono::milliseconds(3)); - } - - 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(); + 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 @@ -521,98 +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(); + 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_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index f56fc338..ce2825c1 100644 --- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp +++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp @@ -147,20 +147,20 @@ TEST_F(BaseEnvFixture, LoadPlugin) { 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 +200,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,7 +234,7 @@ TEST_F(BaseEnvFixture, FailedLoad) EXPECT_FALSE(test_plugin->ran_on_geom_changed_cb.load()); } - env.shutdown(); + env_ptr->shutdown(); } TEST_F(BaseEnvFixture, FailedLoadRecoverReload) @@ -242,24 +242,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) { @@ -269,18 +269,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) @@ -289,24 +289,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) { @@ -314,19 +314,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(); + 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 982e7e42..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,7 +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(); + 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; From fdbc397b521df36e6d3f6fd3b3b0c06e4983a5ef Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 25 Nov 2024 20:34:57 +0100 Subject: [PATCH 07/17] test: adds initial offscreen rendering tests --- mujoco_ros/test/CMakeLists.txt | 18 + mujoco_ros/test/camera_world.xml | 45 ++ mujoco_ros/test/launch/mujoco_render.test | 10 + mujoco_ros/test/mujoco_env_fixture.h | 2 + mujoco_ros/test/mujoco_render_test.cpp | 808 ++++++++++++++++++++++ 5 files changed, 883 insertions(+) create mode 100644 mujoco_ros/test/camera_world.xml create mode 100644 mujoco_ros/test/launch/mujoco_render.test create mode 100644 mujoco_ros/test/mujoco_render_test.cpp 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..12733b1b --- /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 c0bcb07f..1e80f724 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); } diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp new file mode 100644 index 00000000..1324a2e6 --- /dev/null +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -0,0 +1,808 @@ +/********************************************************************* + * 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; + +std::vector rgb_images; +std::vector depth_images; +std::vector seg_images; +std::vector cam_infos; + +// callbacks to save an image in a buffer +void rgb_cb(const sensor_msgs::Image::ConstPtr &msg) +{ + rgb_images.emplace_back(*msg); +} +void depth_cb(const sensor_msgs::Image::ConstPtr &msg) +{ + depth_images.emplace_back(*msg); +} +void seg_cb(const sensor_msgs::Image::ConstPtr &msg) +{ + seg_images.emplace_back(*msg); +} +void cam_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +{ + cam_infos.emplace_back(*msg); +} + +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 defined(USE_GLFW) || defined(USE_EGL) || defined(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_Topic_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 found = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + found = true; + break; + } + } + EXPECT_TRUE(found); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_Topic_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 found = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + found = true; + break; + } + } + EXPECT_TRUE(found); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, SEGMENTATION_Topic_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 found = false; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + found = true; + break; + } + } + EXPECT_TRUE(found); + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_DEPTH_Topic_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; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + found_depth = true; + } + if (found_rgb && found_depth) + break; + } + EXPECT_TRUE(found_rgb && found_depth); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topic_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; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + found_seg = true; + } + if (found_rgb && found_seg) + break; + } + EXPECT_TRUE(found_rgb && found_seg); + + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topic_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; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + found_seg = true; + } + if (found_depth && found_seg) + break; + } + EXPECT_TRUE(found_depth && found_seg); + env_ptr->shutdown(); +} + +TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topic_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; + for (const auto &t : master_topics) { + if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { + found_rgb = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { + found_depth = true; + } else if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { + found_seg = true; + } + if (found_rgb && found_depth && found_seg) + break; + } + EXPECT_TRUE(found_rgb && found_depth && found_seg); + env_ptr->shutdown(); + + 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, 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", 72); + nh->setParam("cam_config/test_cam/height", 48); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // Clear image buffers + rgb_images.clear(); + cam_infos.clear(); + + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); + ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + + 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_, 30); + EXPECT_EQ(offscreen->cams[0]->width_, 72); + EXPECT_EQ(offscreen->cams[0]->height_, 48); + + env_ptr->step(1); + // Wait for image to be published with 100ms timeout + float seconds = 0; + while (rgb_images.size() == 0 && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; + + ASSERT_EQ(cam_infos.size(), 1); + ASSERT_EQ(rgb_images.size(), 1); + + ros::Time t1 = ros::Time::now(); + EXPECT_EQ(rgb_images[0].header.stamp, t1); + EXPECT_EQ(rgb_images[0].header.frame_id, "test_cam_optical_frame"); + EXPECT_EQ(rgb_images[0].width, 72); + EXPECT_EQ(rgb_images[0].height, 48); + EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); + + EXPECT_EQ(cam_infos[0].header.stamp, t1); + + rgb_images.clear(); + cam_infos.clear(); + + 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", 72); + nh->setParam("cam_config/test_cam/height", 48); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // Clear image buffers + rgb_images.clear(); + cam_infos.clear(); + + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); + ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + + env_ptr->startWithXML(xml_path); + + EXPECT_TRUE(env_ptr->settings_.headless); + EXPECT_TRUE(env_ptr->settings_.render_offscreen); + + env_ptr->step(1); + // Wait for image to be published with 100ms timeout + float seconds = 0; + while (rgb_images.size() == 0 && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; + + 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 100ms timeout + seconds = 0; + while (rgb_images.size() == 0 && seconds < 0.1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; + + ASSERT_EQ(cam_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(); + ROS_INFO_STREAM("Next pub time will be " << (1.0 / 30.0) << " seconds from now. Thus in " + << std::ceil((1.0 / 30.0) / m->opt.timestep) << " steps."); + 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(cam_infos.size(), 1); + ASSERT_EQ(rgb_images.size(), 1); + + env_ptr->step(1); + // Wait for image to be published with 100ms timeout + seconds = 0; + while (rgb_images.size() < 2 && seconds < 0.1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + // should now have received new image + EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; + + ASSERT_EQ(cam_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(cam_infos[0].header.stamp, t1); + ASSERT_EQ(cam_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(cam_infos.size(), 3); + ASSERT_EQ(rgb_images.size(), 3); + + // Check that the timestamps are as expected + EXPECT_EQ(rgb_images[2].header.stamp, cam_infos[2].header.stamp); + EXPECT_EQ(rgb_images[2].header.stamp, t3); + EXPECT_EQ(cam_infos[2].header.stamp, t3); + + rgb_images.clear(); + cam_infos.clear(); + + 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", 72); + nh->setParam("cam_config/test_cam/height", 48); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // Clear image buffers + rgb_images.clear(); + + // Subscribe to topic + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); + + 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_, 72); + EXPECT_EQ(offscreen->cams[0]->height_, 48); + + // Wait for image to be published with 100ms timeout + float seconds = 0; + while (rgb_images.size() == 0 && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; + + ASSERT_EQ(rgb_images.size(), 1); + EXPECT_EQ(rgb_images[0].data.size(), 72 * 48 * 3); + EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); + + rgb_images.clear(); + + 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", 72); + nh->setParam("cam_config/test_cam/height", 48); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // Clear image buffers + depth_images.clear(); + + // Subscribe to topic + ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth", 1, depth_cb); + + 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_, 72); + EXPECT_EQ(offscreen->cams[0]->height_, 48); + + // Wait for image to be published with 100ms timeout + float seconds = 0; + while (depth_images.size() == 0 && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, .1) << "Depth image not published within 100ms"; + + ASSERT_EQ(depth_images.size(), 1); + EXPECT_EQ(depth_images[0].width, 72); + EXPECT_EQ(depth_images[0].height, 48); + EXPECT_EQ(depth_images[0].encoding, sensor_msgs::image_encodings::TYPE_32FC1); + + depth_images.clear(); + 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", 72); + nh->setParam("cam_config/test_cam/height", 48); + + std::string xml_path = ros::package::getPath("mujoco_ros") + "/test/camera_world.xml"; + env_ptr = std::make_unique(""); + + // Clear image buffers + seg_images.clear(); + + // Subscribe to topic + ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented", 1, seg_cb); + + 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_, 72); + EXPECT_EQ(offscreen->cams[0]->height_, 48); + + // Wait for image to be published with 100ms timeout + float seconds = 0; + while (seg_images.size() == 0 && seconds < .1) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + seconds += 0.001; + } + EXPECT_LT(seconds, .1) << "Segmentation image not published within 100ms"; + + ASSERT_EQ(seg_images.size(), 1); + EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 1); + + seg_images.clear(); + env_ptr->shutdown(); +} + +#endif // defined(USE_GLFW) || defined(USE_EGL) || defined(USE_OSMESA) // i.e. any render backend available + +#if !defined(USE_GLFW) && !defined(USE_EGL) && !defined(USE_OSMESA) // 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.size() == 0); + + env_ptr->shutdown(); +} +#endif // !defined(USE_GLFW) && !defined(USE_EGL) && !defined(USE_OSMESA) // i.e. no render backend available + +// TODOs: +// - Tests for offscreen rendering +// + Test if RGB topic is available (done) +// + Test if Depth topic is available (done) +// + Test if Segmentation topic is available (done) +// + Test if CameraInfo topic is available +// + Test default camera settings +// + Resolution settings +// + test all combinations of RGB, Depth, and Segmentation (done) +// + Test that camera timings are correct by stepping through the simulation and checking the timestamps +// + Test camera image data types From 435fd52b470644f9efa2ba9b7b939e110b3551f0 Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 25 Nov 2024 21:08:34 +0100 Subject: [PATCH 08/17] gh: adds build deps for EGL/OSMESA to ci docker --- .docker/Dockerfile | 1 + 1 file changed, 1 insertion(+) 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 \ From 1418446c4c3c8d919d181a24efbaaecc501cb1ff Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 25 Nov 2024 21:18:51 +0100 Subject: [PATCH 09/17] gh: adds workflow combinations for render backend --- .github/workflows/ci.yaml | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1da027ed..a0da9731 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'}}" + -D${{ matrix.render_backend }}=ON 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 From 63ca74bb0684c17d2e4d70c81aaa603e34f99f2d Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 28 Nov 2024 15:56:51 +0100 Subject: [PATCH 10/17] fix: segmentation image rendering --- CHANGELOG.md | 2 ++ mujoco_ros/src/offscreen_camera.cpp | 53 ++++++++++------------------- 2 files changed, 20 insertions(+), 35 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 641174d3..ca46d0d7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c * 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). @@ -28,6 +29,7 @@ Loading and reset times are reported in the server debug log. All plugin stats c * 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/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index 3f03ea7a..ca86c05c 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -165,11 +165,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,18 +183,18 @@ 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); } #ifdef USE_GLFW glfwSwapBuffers(offscreen->window.get()); #endif - 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"; @@ -266,38 +264,23 @@ void OffscreenCamera::renderAndPublish(mujoco_ros::OffscreenRenderContext *offsc last_pub_ = ros::Time(scn_state_.data.time); bool rendered = false; - 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; + bool rgb = stream_type_ & streamType::RGB && rgb_pub_.getNumSubscribers() > 0; + bool depth = stream_type_ & streamType::DEPTH && depth_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); + rendered = 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); + rendered = renderAndPubIfNecessary(offscreen, false, false, true) || rendered; + } else { // render maybe RGB/SEGMENTED and maybe DEPTH + offscreen->scn.flags[mjRND_SEGMENT] = segment; + rendered = renderAndPubIfNecessary(offscreen, rgb, depth, segment); } if (rendered) { From 6f01c4e681fa7f3625e7b2608b6de5e653cfd9a6 Mon Sep 17 00:00:00 2001 From: lruegeme Date: Fri, 15 Nov 2024 17:05:57 +0100 Subject: [PATCH 11/17] syncronize and split camera_info with images. - give each image from cameras its own topic - add configuration for camera topics This changes cameras to publish a syncronized camera_info for each image. e.g for rgb image: /mujoco_server/cameras/camera_name/rgb/camera_info /mujoco_server/cameras/camera_name/rgb/image_raw Additional configuration options to change topics: camera_name/topic: defaults to "/mujoco_server/cameras/camera_name" camera_name/name_rgb: default to "rbg" camera_name/name_depth: default to "depth" camera_name/name_segment: default to "segmented" --- .../include/mujoco_ros/offscreen_camera.h | 22 ++++--- mujoco_ros/src/offscreen_camera.cpp | 66 ++++++++++--------- mujoco_ros/src/offscreen_rendering.cpp | 21 ++++-- 3 files changed, 65 insertions(+), 44 deletions(-) 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/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index ca86c05c..fbdec58a 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -47,10 +47,12 @@ 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) , width_(width) @@ -58,9 +60,10 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &cam_na , 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 +71,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 +126,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 +159,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) @@ -194,11 +199,16 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext 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_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 +226,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 +237,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 +261,9 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext } depth_pub_.publish(depth_msg); + depth_camera_info_pub_.publish(camera_info_msg); } + return true; } @@ -261,12 +275,14 @@ 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 && segment_pub_.getNumSubscribers() > 0; - bool rgb = stream_type_ & streamType::RGB && rgb_pub_.getNumSubscribers() > 0; - bool depth = stream_type_ & streamType::DEPTH && depth_pub_.getNumSubscribers() > 0; + 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_; @@ -274,26 +290,14 @@ void OffscreenCamera::renderAndPublish(mujoco_ros::OffscreenRenderContext *offsc // flag). if (rgb && segment) { // first render RGB and maybe DEPTH, then SEGMENTED offscreen->scn.flags[mjRND_SEGMENT] = 0; - rendered = renderAndPubIfNecessary(offscreen, true, depth, false); + renderAndPubIfNecessary(offscreen, true, depth, false); offscreen->scn.flags[mjRND_SEGMENT] = 1; - rendered = renderAndPubIfNecessary(offscreen, false, false, true) || rendered; + renderAndPubIfNecessary(offscreen, false, false, true); } else { // render maybe RGB/SEGMENTED and maybe DEPTH offscreen->scn.flags[mjRND_SEGMENT] = segment; - rendered = renderAndPubIfNecessary(offscreen, rgb, depth, segment); - } - - if (rendered) { - publishCameraInfo(); + renderAndPubIfNecessary(offscreen, rgb, depth, segment); } } -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 1e2d1743..107318cd 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -86,10 +86,9 @@ OffscreenRenderContext::~OffscreenRenderContext() 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; @@ -124,19 +123,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) { From d05fd0fec72dfdfcf833f1b16fd9d07affcf5c2b Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 28 Nov 2024 17:23:45 +0100 Subject: [PATCH 12/17] fix: sets off-cam topic member correctly on init --- mujoco_ros/src/offscreen_camera.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mujoco_ros/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index fbdec58a..49e34884 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -55,6 +55,7 @@ OffscreenCamera::OffscreenCamera(const uint8_t cam_id, const std::string &base_t mujoco_ros::MujocoEnv *env_ptr) : cam_id_(cam_id) , cam_name_(cam_name) + , topic_(base_topic) , width_(width) , height_(height) , stream_type_(stream_type) From 6bc688846aa110ee92e4cc69161ee629859864e9 Mon Sep 17 00:00:00 2001 From: David Leins Date: Thu, 28 Nov 2024 17:47:39 +0100 Subject: [PATCH 13/17] test: adapt render tests to additional params --- mujoco_ros/test/mujoco_render_test.cpp | 459 +++++++++++++++++++------ 1 file changed, 346 insertions(+), 113 deletions(-) diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp index 1324a2e6..90a90e88 100644 --- a/mujoco_ros/test/mujoco_render_test.cpp +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -76,9 +76,13 @@ using namespace mujoco_ros; namespace mju = ::mujoco::sample_util; std::vector rgb_images; +std::vector rgb_infos; + std::vector depth_images; +// std::vector depth_infos; + std::vector seg_images; -std::vector cam_infos; +// std::vector seg_infos; // callbacks to save an image in a buffer void rgb_cb(const sensor_msgs::Image::ConstPtr &msg) @@ -93,10 +97,18 @@ void seg_cb(const sensor_msgs::Image::ConstPtr &msg) { seg_images.emplace_back(*msg); } -void cam_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +void rgb_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) { - cam_infos.emplace_back(*msg); + rgb_infos.emplace_back(*msg); } +// void depth_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +// { +// depth_infos.emplace_back(*msg); +// } +// void seg_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) +// { +// seg_infos.emplace_back(*msg); +// } TEST_F(BaseEnvFixture, Not_Headless_Warn) { @@ -148,7 +160,7 @@ TEST_F(BaseEnvFixture, Headless_params_correct) env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_Topic_Available) +TEST_F(BaseEnvFixture, RGB_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -170,19 +182,22 @@ TEST_F(BaseEnvFixture, RGB_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/rgb") { - found = true; - break; + 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(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, DEPTH_Topic_Available) +TEST_F(BaseEnvFixture, DEPTH_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -204,19 +219,22 @@ TEST_F(BaseEnvFixture, DEPTH_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/depth") { - found = true; - break; + 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(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -238,18 +256,21 @@ TEST_F(BaseEnvFixture, SEGMENTATION_Topic_Available) ros::master::V_TopicInfo master_topics; ros::master::getTopics(master_topics); - bool found = false; + bool img = false, info = false; for (const auto &t : master_topics) { - if (t.name == env_ptr->getHandleNamespace() + "/cameras/test_cam/segmented") { - found = true; - break; + 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(found); + EXPECT_TRUE(img && info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_DEPTH_Topic_Available) +TEST_F(BaseEnvFixture, RGB_DEPTH_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -272,21 +293,26 @@ TEST_F(BaseEnvFixture, RGB_DEPTH_Topic_Available) 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") { + 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") { + } 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) + if (found_rgb && found_depth && found_rgb_info && found_depth_info) break; } - EXPECT_TRUE(found_rgb && found_depth); + EXPECT_TRUE(found_rgb && found_depth && found_rgb_info && found_depth_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -309,21 +335,26 @@ TEST_F(BaseEnvFixture, RGB_SEGMENTATION_Topic_Available) 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") { + 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") { + } 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) + if (found_rgb && found_seg && found_rgb_info && found_seg_info) break; } - EXPECT_TRUE(found_rgb && found_seg); + EXPECT_TRUE(found_rgb && found_seg && found_rgb_info && found_seg_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -346,20 +377,25 @@ TEST_F(BaseEnvFixture, DEPTH_SEGMENTATION_Topic_Available) 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") { + 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") { + } 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) + if (found_depth && found_seg && found_depth_info && found_seg_info) break; } - EXPECT_TRUE(found_depth && found_seg); + EXPECT_TRUE(found_depth && found_seg && found_depth_info && found_seg_info); env_ptr->shutdown(); } -TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topic_Available) +TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topics_Available) { nh->setParam("no_render", false); nh->setParam("headless", true); @@ -382,19 +418,25 @@ TEST_F(BaseEnvFixture, RGB_DEPTH_SEGMENTATION_Topic_Available) 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") { + 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") { + } 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") { + } 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) + 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); - env_ptr->shutdown(); + EXPECT_TRUE(found_rgb && found_depth && found_seg && found_rgb_info && found_depth_info && found_seg_info); env_ptr->shutdown(); } @@ -454,6 +496,206 @@ TEST_F(BaseEnvFixture, Resolution_Settings) 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); @@ -468,51 +710,50 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) // Clear image buffers rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb); + ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/rgb/camera_info", 1, rgb_info_cb); 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); - // Check default camera settings + 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]->stream_type_, rendering::streamType::RGB); - EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30); - EXPECT_EQ(offscreen->cams[0]->width_, 72); - EXPECT_EQ(offscreen->cams[0]->height_, 48); + EXPECT_EQ(offscreen->cams[0]->rgb_pub_.getNumSubscribers(), 1); - env_ptr->step(1); - // Wait for image to be published with 100ms timeout - float seconds = 0; - while (rgb_images.size() == 0 && seconds < .1) { + // Wait for image to be published with 400ms timeout + float seconds = 0.f; + while (rgb_images.size() == 0 && rgb_infos.size() == 0 && seconds < .4f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; + seconds += 0.001f; } - EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; - ASSERT_EQ(cam_infos.size(), 1); + EXPECT_EQ(offscreen->cams[0]->stream_type_, rendering::streamType::RGB); + EXPECT_EQ(offscreen->cams[0]->pub_freq_, 30); + + ASSERT_EQ(rgb_infos.size(), 1); ASSERT_EQ(rgb_images.size(), 1); ros::Time t1 = ros::Time::now(); EXPECT_EQ(rgb_images[0].header.stamp, t1); - EXPECT_EQ(rgb_images[0].header.frame_id, "test_cam_optical_frame"); + EXPECT_STREQ(rgb_images[0].header.frame_id.c_str(), "test_cam_optical_frame"); EXPECT_EQ(rgb_images[0].width, 72); EXPECT_EQ(rgb_images[0].height, 48); EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); - EXPECT_EQ(cam_infos[0].header.stamp, t1); + EXPECT_EQ(rgb_infos[0].header.stamp, t1); rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); env_ptr->shutdown(); } @@ -531,11 +772,11 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // Clear image buffers rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/camera_info", 1, cam_info_cb); + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb); + ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/rgb/camera_info", 1, rgb_info_cb); env_ptr->startWithXML(xml_path); @@ -543,13 +784,6 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) EXPECT_TRUE(env_ptr->settings_.render_offscreen); env_ptr->step(1); - // Wait for image to be published with 100ms timeout - float seconds = 0; - while (rgb_images.size() == 0 && seconds < .1) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; - } - EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); ASSERT_EQ(offscreen->cams.size(), 1); @@ -560,22 +794,20 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) 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 100ms timeout - seconds = 0; - while (rgb_images.size() == 0 && seconds < 0.1) { + // Wait for image to be published with 400ms timeout + float seconds = 0.f; + while (rgb_images.size() == 0 && rgb_infos.size() == 0 && seconds < .4f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; + seconds += 0.001f; } - EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; - ASSERT_EQ(cam_infos.size(), 1); + 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(); - ROS_INFO_STREAM("Next pub time will be " << (1.0 / 30.0) << " seconds from now. Thus in " - << std::ceil((1.0 / 30.0) / m->opt.timestep) << " steps."); + 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); @@ -583,28 +815,28 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) std::this_thread::sleep_for(std::chrono::milliseconds(5)); // should not have received a new image yet - ASSERT_EQ(cam_infos.size(), 1); + ASSERT_EQ(rgb_infos.size(), 1); ASSERT_EQ(rgb_images.size(), 1); env_ptr->step(1); - // Wait for image to be published with 100ms timeout - seconds = 0; - while (rgb_images.size() < 2 && seconds < 0.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.001; + seconds += 0.001f; } // should now have received new image - EXPECT_LT(seconds, 0.1) << "RGB image not published within 100ms"; + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; - ASSERT_EQ(cam_infos.size(), 2); + 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(cam_infos[0].header.stamp, t1); - ASSERT_EQ(cam_infos[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); @@ -612,16 +844,16 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // Step over next image trigger but before trigger after that env_ptr->step(2 * n_steps - 1); - ASSERT_EQ(cam_infos.size(), 3); + 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, cam_infos[2].header.stamp); + EXPECT_EQ(rgb_images[2].header.stamp, rgb_infos[2].header.stamp); EXPECT_EQ(rgb_images[2].header.stamp, t3); - EXPECT_EQ(cam_infos[2].header.stamp, t3); + EXPECT_EQ(rgb_infos[2].header.stamp, t3); rgb_images.clear(); - cam_infos.clear(); + rgb_infos.clear(); env_ptr->shutdown(); } @@ -641,7 +873,7 @@ TEST_F(BaseEnvFixture, RGB_Image_Dtype) rgb_images.clear(); // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb", 1, rgb_cb); + ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb); env_ptr->startWithXML(xml_path); @@ -657,13 +889,13 @@ TEST_F(BaseEnvFixture, RGB_Image_Dtype) EXPECT_EQ(offscreen->cams[0]->width_, 72); EXPECT_EQ(offscreen->cams[0]->height_, 48); - // Wait for image to be published with 100ms timeout - float seconds = 0; - while (rgb_images.size() == 0 && seconds < .1) { + // Wait for image to be published with 400ms timeout + float seconds = 0.f; + while (rgb_images.size() == 0 && seconds < .4f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; + seconds += 0.001f; } - EXPECT_LT(seconds, .1) << "RGB image not published within 100ms"; + EXPECT_LT(seconds, .4f) << "RGB image not published within 400ms"; ASSERT_EQ(rgb_images.size(), 1); EXPECT_EQ(rgb_images[0].data.size(), 72 * 48 * 3); @@ -690,7 +922,7 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) depth_images.clear(); // Subscribe to topic - ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth", 1, depth_cb); + ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth/image_raw", 1, depth_cb); env_ptr->startWithXML(xml_path); @@ -706,13 +938,13 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) EXPECT_EQ(offscreen->cams[0]->width_, 72); EXPECT_EQ(offscreen->cams[0]->height_, 48); - // Wait for image to be published with 100ms timeout - float seconds = 0; - while (depth_images.size() == 0 && seconds < .1) { + // Wait for image to be published with 200ms timeout + float seconds = 0.f; + while (depth_images.size() == 0 && seconds < .2f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; + seconds += 0.001f; } - EXPECT_LT(seconds, .1) << "Depth image not published within 100ms"; + EXPECT_LT(seconds, .2f) << "Depth image not published within 200ms"; ASSERT_EQ(depth_images.size(), 1); EXPECT_EQ(depth_images[0].width, 72); @@ -739,7 +971,7 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) seg_images.clear(); // Subscribe to topic - ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented", 1, seg_cb); + ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented/image_raw", 1, seg_cb); env_ptr->startWithXML(xml_path); @@ -755,16 +987,17 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) EXPECT_EQ(offscreen->cams[0]->width_, 72); EXPECT_EQ(offscreen->cams[0]->height_, 48); - // Wait for image to be published with 100ms timeout - float seconds = 0; - while (seg_images.size() == 0 && seconds < .1) { + // Wait for image to be published with 200ms timeout + float seconds = 0.f; + while (seg_images.size() == 0 && seconds < .2f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); - seconds += 0.001; + seconds += 0.001f; } - EXPECT_LT(seconds, .1) << "Segmentation image not published within 100ms"; + EXPECT_LT(seconds, .2f) << "Segmentation image not published within 200ms"; ASSERT_EQ(seg_images.size(), 1); - EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 1); + EXPECT_EQ(seg_images[0].data.size(), 72 * 48 * 3); + EXPECT_EQ(seg_images[0].encoding, sensor_msgs::image_encodings::RGB8); seg_images.clear(); env_ptr->shutdown(); From 28cb4b57317b912d641b6a7a0b0a15afb20a7f81 Mon Sep 17 00:00:00 2001 From: David Leins Date: Fri, 29 Nov 2024 20:26:56 +0100 Subject: [PATCH 14/17] chg: render backend cmake changes --- .github/workflows/ci.yaml | 2 +- mujoco_ros/CMakeLists.txt | 42 +++------- mujoco_ros/cmake/ConfigureRenderBackend.cmake | 79 +++++++++++++++++++ mujoco_ros/cmake/GenerateBackendHeader.cmake | 6 ++ mujoco_ros/cmake/render_backend.h.in | 8 ++ mujoco_ros/include/mujoco_ros/mujoco_env.h | 13 +-- mujoco_ros/src/main.cpp | 8 +- mujoco_ros/src/mujoco_env.cpp | 14 ++-- mujoco_ros/src/offscreen_camera.cpp | 2 +- mujoco_ros/src/offscreen_rendering.cpp | 20 ++--- mujoco_ros/test/mujoco_render_test.cpp | 10 ++- mujoco_ros/test/mujoco_ros_plugin_test.cpp | 22 ++++-- 12 files changed, 156 insertions(+), 70 deletions(-) create mode 100644 mujoco_ros/cmake/ConfigureRenderBackend.cmake create mode 100644 mujoco_ros/cmake/GenerateBackendHeader.cmake create mode 100644 mujoco_ros/cmake/render_backend.h.in diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a0da9731..ff54530f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -65,7 +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'}}" - -D${{ matrix.render_backend }}=ON + -DRENDER_BACKEND=${{ matrix.render_backend }} UPSTREAM_CMAKE_ARGS: -DCMAKE_CXX_FLAGS= -DCMAKE_CXX_STANDARD=17 CCACHE_DIR: ${{ github.workspace }}/.ccache diff --git a/mujoco_ros/CMakeLists.txt b/mujoco_ros/CMakeLists.txt index 5bff9680..e1f97ace 100644 --- a/mujoco_ros/CMakeLists.txt +++ b/mujoco_ros/CMakeLists.txt @@ -18,13 +18,15 @@ 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) -option(NO_GLFW "Disable GLFW3 support and try finding OpenGL EGL or OSMESA for offscreen rendering" OFF) -option(USE_EGL "Build with EGL enabled offscreen rendering. Entails `NO_GLFW`" OFF) -option(USE_OSMESA "Build with OSMESA enabled offscreen rendering. Entails `NO_GLFW`" OFF) - # Find catkin macros and libraries # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) # is used, also find other catkin packages @@ -64,35 +66,6 @@ configure_project_option( # Find MuJoCo find_package(mujoco 3.2.0 REQUIRED) -find_library(GLFW libglfw.so.3) # Find GLFW3 for GUI - - -if (NO_GLFW OR USE_EGL OR USE_OSMESA 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 (USE_OSMESA OR ${OpenGL_EGL_FOUND} STREQUAL "FALSE") - message(WARNING "EGL not found or disabled. Falling back to OSMESA.") - - find_package(OSMesa) - - if (!OSMesa_FOUND) - message(WARNING "EGL disabled or not found and OSMesa could not be found. Offscreen rendering will not be available!") - 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() - # ############################################### # # Declare ROS dynamic reconfigure parameters ## @@ -112,6 +85,7 @@ endif() generate_dynamic_reconfigure_options( cfg/SimParams.cfg ) +include(ConfigureRenderBackend) # ################################## # # catkin specific configuration ## @@ -148,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/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/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 7b0c6fde..0e86db90 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,10 +91,10 @@ #include -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW #include #include -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA #include #endif @@ -120,9 +121,9 @@ struct OffscreenRenderContext mjvCamera cam; std::unique_ptr rgb; std::unique_ptr depth; -#ifdef USE_GLFW +#if RENDER_BACKEND == USE_GLFW std::shared_ptr window; -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA struct { OSMesaContext ctx; @@ -280,11 +281,11 @@ class MujocoEnv bool togglePaused(bool paused, const std::string &admin_hash = std::string()); -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW GlfwAdapter *gui_adapter_ = nullptr; #endif -#if defined(USE_EGL) || defined(USE_OSMESA) +#if RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA bool InitGL(); #endif diff --git a/mujoco_ros/src/main.cpp b/mujoco_ros/src/main.cpp index f565f072..2b606813 100644 --- a/mujoco_ros/src/main.cpp +++ b/mujoco_ros/src/main.cpp @@ -34,7 +34,8 @@ /* Authors: David P. Leins */ -#include +#include + #include #include @@ -44,8 +45,9 @@ #include #include -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW #include +#include #endif namespace { @@ -150,7 +152,7 @@ int main(int argc, char **argv) env->startPhysicsLoop(); env->startEventLoop(); -#ifdef USE_GLFW +#if RENDER_BACKEND == USE_GLFW if (!env->settings_.headless) { ROS_INFO("Launching viewer"); auto viewer = std::make_unique( diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index dc418a44..645c9393 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -43,11 +43,11 @@ #include #include -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW static std::string render_backend = "GLFW"; -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA static std::string render_backend = "OSMESA"; -#elif defined(USE_EGL) +#elif RENDER_BACKEND == USE_EGL static std::string render_backend = "EGL"; #else static std::string render_backend = "NONE. No offscreen rendering available."; @@ -59,7 +59,7 @@ namespace mju = ::mujoco::sample_util; using Seconds = std::chrono::duration; using Milliseconds = std::chrono::duration; -#ifdef USE_GLFW +#if RENDER_BACKEND == USE_GLFW namespace { int MaybeGlfwInit() { @@ -137,7 +137,7 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) nh_->param("headless", settings_.headless, true); if (!settings_.headless) { -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW gui_adapter_ = new mujoco_ros::GlfwAdapter(); #else ROS_ERROR("Compiled without GLFW support. Cannot run in non-headless mode."); @@ -160,10 +160,10 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) if (settings_.render_offscreen) { bool can_render = true; -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW can_render = MaybeGlfwInit(); ROS_ERROR_COND(!can_render, "Failed to initialize GLFW. Cannot render offscreen!"); -#elif !defined(USE_EGL) && !defined(USE_OSMESA) +#elif RENDER_BACKEND == NONE ROS_ERROR("No rendering backend available. Cannot render offscreen!"); can_render = false; #endif diff --git a/mujoco_ros/src/offscreen_camera.cpp b/mujoco_ros/src/offscreen_camera.cpp index 49e34884..c2bd10d8 100644 --- a/mujoco_ros/src/offscreen_camera.cpp +++ b/mujoco_ros/src/offscreen_camera.cpp @@ -196,7 +196,7 @@ bool OffscreenCamera::renderAndPubIfNecessary(mujoco_ros::OffscreenRenderContext } else { // depth only mjr_readPixels(nullptr, offscreen->depth.get(), viewport, &offscreen->con); } -#ifdef USE_GLFW +#if RENDER_BACKEND == USE_GLFW glfwSwapBuffers(offscreen->window.get()); #endif diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 107318cd..43bb9622 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -40,7 +40,7 @@ #include -#ifdef USE_EGL +#if RENDER_BACKEND == USE_EGL #include #endif @@ -48,7 +48,7 @@ namespace mujoco_ros { OffscreenRenderContext::~OffscreenRenderContext() { -#if defined(USE_GLFW) +#if RENDER_BACKEND == USE_GLFW if (window != nullptr) { ROS_DEBUG("Freeing GLFW offscreen context"); std::unique_lock lock(render_mutex); @@ -56,7 +56,7 @@ OffscreenRenderContext::~OffscreenRenderContext() mjr_defaultContext(&con); mjr_freeContext(&con); } -#elif defined(USE_EGL) +#elif RENDER_BACKEND == USE_EGL ROS_DEBUG("Freeing EGL offscreen context"); mjr_defaultContext(&con); mjr_freeContext(&con); @@ -76,7 +76,7 @@ OffscreenRenderContext::~OffscreenRenderContext() // Terminate display eglTerminate(display); } -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA ROS_DEBUG("Freeing OSMESA offscreen context"); mjr_defaultContext(&con); mjr_freeContext(&con); @@ -164,7 +164,7 @@ void MujocoEnv::initializeRenderResources() ROS_DEBUG_NAMED("offscreen_rendering", "Initializing offscreen rendering utils"); -#ifdef USE_GLFW +#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); @@ -176,7 +176,7 @@ void MujocoEnv::initializeRenderResources() mjr_setBuffer(mjFB_OFFSCREEN, &offscreen_.con); } -#if defined(USE_EGL) +#if RENDER_BACKEND == USE_EGL bool MujocoEnv::InitGL() { ROS_DEBUG("Initializing EGL..."); @@ -238,7 +238,7 @@ bool MujocoEnv::InitGL() ROS_DEBUG("EGL initialized"); return true; } -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA bool MujocoEnv::InitGL() { ROS_DEBUG("Initializing OSMesa..."); @@ -262,7 +262,7 @@ bool MujocoEnv::InitGL() void MujocoEnv::offscreenRenderLoop() { -#if defined(USE_GLFW) +#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), @@ -278,12 +278,12 @@ void MujocoEnv::offscreenRenderLoop() Glfw().glfwMakeContextCurrent(offscreen_.window.get()); Glfw().glfwSwapInterval(0); -#elif defined(USE_EGL) +#elif RENDER_BACKEND == USE_EGL if (!InitGL()) { ROS_ERROR("Failed to initialize EGL. Cannot run offscreen rendering"); return; } -#elif defined(USE_OSMESA) +#elif RENDER_BACKEND == USE_OSMESA if (!InitGL()) { ROS_ERROR("Failed to initialize OSMesa. Cannot run offscreen rendering"); return; diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp index 90a90e88..25608876 100644 --- a/mujoco_ros/test/mujoco_render_test.cpp +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -125,7 +125,8 @@ TEST_F(BaseEnvFixture, Not_Headless_Warn) env_ptr->shutdown(); } -#if defined(USE_GLFW) || defined(USE_EGL) || defined(USE_OSMESA) // i.e. any render backend available +#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); @@ -1003,9 +1004,10 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) env_ptr->shutdown(); } -#endif // defined(USE_GLFW) || defined(USE_EGL) || defined(USE_OSMESA) // i.e. any render backend available +#endif // RENDER_BACKEND == USE_GLFW || RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA // i.e. any render + // backend available -#if !defined(USE_GLFW) && !defined(USE_EGL) && !defined(USE_OSMESA) // i.e. no 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); @@ -1026,7 +1028,7 @@ TEST_F(BaseEnvFixture, No_Render_Backend_Headless_Warn) env_ptr->shutdown(); } -#endif // !defined(USE_GLFW) && !defined(USE_EGL) && !defined(USE_OSMESA) // i.e. no render backend available +#endif // RENDER_BACKEND == USE_NONE // i.e. no render backend available // TODOs: // - Tests for offscreen rendering diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index ce2825c1..15eb2893 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,7 @@ class LoadedPluginFixture : public ::testing::Test { nh = std::make_unique("~"); nh->setParam("unpause", false); - nh->setParam("no_x", true); + nh->setParam("headless", true); nh->setParam("use_sim_time", true); env_ptr = new MujocoEnvTestWrapper(); @@ -124,10 +125,21 @@ 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(LoadedPluginFixture, RenderCallback) +{ + EXPECT_TRUE(env_ptr->step()); + EXPECT_TRUE(test_plugin->ran_render_cb.load()); +} +#endif + +#if RENDER_BACKEND == NONE +TEST_F(LoadedPluginFixture, RenderCallback_NoRender) +{ + EXPECT_TRUE(env_ptr->step()); + EXPECT_FALSE(test_plugin->ran_render_cb.load()); +} +#endif TEST_F(LoadedPluginFixture, LastCallback) { From 4b7308d2c345cb8408eb3bd6be8710e5cacf299a Mon Sep 17 00:00:00 2001 From: David Leins Date: Fri, 29 Nov 2024 23:17:44 +0100 Subject: [PATCH 15/17] tests: fix tests --- mujoco_ros/src/offscreen_rendering.cpp | 2 +- mujoco_ros/test/launch/mujoco_render.test | 2 +- mujoco_ros/test/mujoco_env_fixture.h | 2 +- mujoco_ros/test/mujoco_env_test.cpp | 2 +- mujoco_ros/test/mujoco_render_test.cpp | 169 +++++++----------- mujoco_ros/test/mujoco_ros_plugin_test.cpp | 81 ++++++++- .../test/mujoco_sensors_test.cpp | 1 + 7 files changed, 141 insertions(+), 118 deletions(-) diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 43bb9622..98beb73e 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -243,7 +243,7 @@ bool MujocoEnv::InitGL() { ROS_DEBUG("Initializing OSMesa..."); // Initialize OSMesa - offscreen_.osmesa.ctx = OSMesaCreateContextExt(GL_RGBA, 24, 8, 8, 0); + offscreen_.osmesa.ctx = OSMesaCreateContextExt(GL_RGBA, 24, 8, 8, nullptr); if (!offscreen_.osmesa.ctx) { ROS_ERROR("OSMesa context creation failed"); return false; diff --git a/mujoco_ros/test/launch/mujoco_render.test b/mujoco_ros/test/launch/mujoco_render.test index 12733b1b..897a9232 100644 --- a/mujoco_ros/test/launch/mujoco_render.test +++ b/mujoco_ros/test/launch/mujoco_render.test @@ -6,5 +6,5 @@ value="$(find mujoco_ros)/config/rosconsole.config"/> - + diff --git a/mujoco_ros/test/mujoco_env_fixture.h b/mujoco_ros/test/mujoco_env_fixture.h index 1e80f724..6ce3c87a 100644 --- a/mujoco_ros/test/mujoco_env_fixture.h +++ b/mujoco_ros/test/mujoco_env_fixture.h @@ -106,7 +106,7 @@ class BaseEnvFixture : public ::testing::Test { protected: std::unique_ptr nh; - std::unique_ptr env_ptr; + std::unique_ptr env_ptr = nullptr; void SetUp() override { diff --git a/mujoco_ros/test/mujoco_env_test.cpp b/mujoco_ros/test/mujoco_env_test.cpp index 1140d373..a42a0506 100644 --- a/mujoco_ros/test/mujoco_env_test.cpp +++ b/mujoco_ros/test/mujoco_env_test.cpp @@ -60,7 +60,7 @@ 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); + EXPECT_THROW(env_ptr = std::make_unique(""), std::runtime_error); } TEST_F(BaseEnvFixture, RunEvalMode) diff --git a/mujoco_ros/test/mujoco_render_test.cpp b/mujoco_ros/test/mujoco_render_test.cpp index 25608876..53f78dc9 100644 --- a/mujoco_ros/test/mujoco_render_test.cpp +++ b/mujoco_ros/test/mujoco_render_test.cpp @@ -75,41 +75,6 @@ int main(int argc, char **argv) using namespace mujoco_ros; namespace mju = ::mujoco::sample_util; -std::vector rgb_images; -std::vector rgb_infos; - -std::vector depth_images; -// std::vector depth_infos; - -std::vector seg_images; -// std::vector seg_infos; - -// callbacks to save an image in a buffer -void rgb_cb(const sensor_msgs::Image::ConstPtr &msg) -{ - rgb_images.emplace_back(*msg); -} -void depth_cb(const sensor_msgs::Image::ConstPtr &msg) -{ - depth_images.emplace_back(*msg); -} -void seg_cb(const sensor_msgs::Image::ConstPtr &msg) -{ - seg_images.emplace_back(*msg); -} -void rgb_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) -{ - rgb_infos.emplace_back(*msg); -} -// void depth_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) -// { -// depth_infos.emplace_back(*msg); -// } -// void seg_info_cb(const sensor_msgs::CameraInfo::ConstPtr &msg) -// { -// seg_infos.emplace_back(*msg); -// } - TEST_F(BaseEnvFixture, Not_Headless_Warn) { nh->setParam("no_render", false); @@ -703,19 +668,22 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) nh->setParam("headless", true); nh->setParam("unpause", false); nh->setParam("cam_config/test_cam/frequency", 30); - nh->setParam("cam_config/test_cam/width", 72); - nh->setParam("cam_config/test_cam/height", 48); + 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(""); - // Clear image buffers - rgb_images.clear(); - rgb_infos.clear(); + 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_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/rgb/camera_info", 1, rgb_info_cb); + 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); @@ -730,32 +698,29 @@ TEST_F(BaseEnvFixture, RGB_Published_Correctly) 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 400ms timeout + // Wait for image to be published with 1s timeout float seconds = 0.f; - while (rgb_images.size() == 0 && rgb_infos.size() == 0 && seconds < .4f) { + 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, .4f) << "RGB image not published within 400ms"; + 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_infos.size(), 1); 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, 72); - EXPECT_EQ(rgb_images[0].height, 48); + 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); - rgb_images.clear(); - rgb_infos.clear(); - env_ptr->shutdown(); } @@ -765,19 +730,22 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) nh->setParam("headless", true); nh->setParam("unpause", false); nh->setParam("cam_config/test_cam/frequency", 30); - nh->setParam("cam_config/test_cam/width", 72); - nh->setParam("cam_config/test_cam/height", 48); + 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(""); - // Clear image buffers - rgb_images.clear(); - rgb_infos.clear(); + 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_cb); - ros::Subscriber info_sub = nh->subscribe("cameras/test_cam/rgb/camera_info", 1, rgb_info_cb); + 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); @@ -797,7 +765,7 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) // Wait for image to be published with 400ms timeout float seconds = 0.f; - while (rgb_images.size() == 0 && rgb_infos.size() == 0 && seconds < .4f) { + while ((rgb_images.empty() || rgb_infos.empty()) && seconds < .4f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001f; } @@ -822,7 +790,7 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) 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) { + while ((rgb_images.size() < 2 || rgb_infos.size() < 2) && seconds < .4f) { std::this_thread::sleep_for(std::chrono::milliseconds(1)); seconds += 0.001f; } @@ -853,9 +821,6 @@ TEST_F(BaseEnvFixture, Cam_Timing_Correct) EXPECT_EQ(rgb_images[2].header.stamp, t3); EXPECT_EQ(rgb_infos[2].header.stamp, t3); - rgb_images.clear(); - rgb_infos.clear(); - env_ptr->shutdown(); } @@ -864,17 +829,17 @@ 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", 72); - nh->setParam("cam_config/test_cam/height", 48); + 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(""); - // Clear image buffers - rgb_images.clear(); - + std::vector rgb_images; // Subscribe to topic - ros::Subscriber rgb_sub = nh->subscribe("cameras/test_cam/rgb/image_raw", 1, rgb_cb); + 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); @@ -887,23 +852,21 @@ TEST_F(BaseEnvFixture, RGB_Image_Dtype) 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_, 72); - EXPECT_EQ(offscreen->cams[0]->height_, 48); + 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.size() == 0 && seconds < .4f) { + 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(), 72 * 48 * 3); + EXPECT_EQ(rgb_images[0].data.size(), 7 * 4 * 3); EXPECT_EQ(rgb_images[0].encoding, sensor_msgs::image_encodings::RGB8); - rgb_images.clear(); - env_ptr->shutdown(); } @@ -913,17 +876,18 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) 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", 72); - nh->setParam("cam_config/test_cam/height", 48); + 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(""); - // Clear image buffers - depth_images.clear(); + std::vector depth_images; // Subscribe to topic - ros::Subscriber depth_sub = nh->subscribe("cameras/test_cam/depth/image_raw", 1, depth_cb); + 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); @@ -936,23 +900,22 @@ TEST_F(BaseEnvFixture, DEPTH_Image_Dtype) 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_, 72); - EXPECT_EQ(offscreen->cams[0]->height_, 48); + 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.size() == 0 && seconds < .2f) { + 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, 72); - EXPECT_EQ(depth_images[0].height, 48); + 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); - depth_images.clear(); env_ptr->shutdown(); } @@ -962,17 +925,18 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) 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", 72); - nh->setParam("cam_config/test_cam/height", 48); + 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(""); - // Clear image buffers - seg_images.clear(); + std::vector seg_images; // Subscribe to topic - ros::Subscriber seg_sub = nh->subscribe("cameras/test_cam/segmented/image_raw", 1, seg_cb); + 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); @@ -985,22 +949,21 @@ TEST_F(BaseEnvFixture, SEGMENTED_Image_Dtype) 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_, 72); - EXPECT_EQ(offscreen->cams[0]->height_, 48); + 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.size() == 0 && seconds < .2f) { + 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(), 72 * 48 * 3); + EXPECT_EQ(seg_images[0].data.size(), 7 * 4 * 3); EXPECT_EQ(seg_images[0].encoding, sensor_msgs::image_encodings::RGB8); - seg_images.clear(); env_ptr->shutdown(); } @@ -1024,20 +987,8 @@ TEST_F(BaseEnvFixture, No_Render_Backend_Headless_Warn) EXPECT_FALSE(env_ptr->settings_.render_offscreen); OffscreenRenderContext *offscreen = env_ptr->getOffscreenContext(); - EXPECT_TRUE(offscreen->cams.size() == 0); + EXPECT_TRUE(offscreen->cams.empty()); env_ptr->shutdown(); } #endif // RENDER_BACKEND == USE_NONE // i.e. no render backend available - -// TODOs: -// - Tests for offscreen rendering -// + Test if RGB topic is available (done) -// + Test if Depth topic is available (done) -// + Test if Segmentation topic is available (done) -// + Test if CameraInfo topic is available -// + Test default camera settings -// + Resolution settings -// + test all combinations of RGB, Depth, and Segmentation (done) -// + Test that camera timings are correct by stepping through the simulation and checking the timestamps -// + Test camera image data types diff --git a/mujoco_ros/test/mujoco_ros_plugin_test.cpp b/mujoco_ros/test/mujoco_ros_plugin_test.cpp index 15eb2893..c334d298 100644 --- a/mujoco_ros/test/mujoco_ros_plugin_test.cpp +++ b/mujoco_ros/test/mujoco_ros_plugin_test.cpp @@ -74,6 +74,7 @@ class LoadedPluginFixture : public ::testing::Test { nh = std::make_unique("~"); nh->setParam("unpause", false); + nh->setParam("no_render", true); nh->setParam("headless", true); nh->setParam("use_sim_time", true); @@ -99,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; @@ -126,18 +129,85 @@ TEST_F(LoadedPluginFixture, PassiveCallback) } #if RENDER_BACKEND == GLFW || RENDER_BACKEND == USE_EGL || RENDER_BACKEND == USE_OSMESA -TEST_F(LoadedPluginFixture, RenderCallback) +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()); - EXPECT_TRUE(test_plugin->ran_render_cb.load()); + + 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(LoadedPluginFixture, RenderCallback_NoRender) +TEST_F(BaseEnvFixture, RenderCallback_NoRender) { - EXPECT_TRUE(env_ptr->step()); - EXPECT_FALSE(test_plugin->ran_render_cb.load()); + 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 @@ -157,6 +227,7 @@ 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"; env_ptr = std::make_unique(""); 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 From 6c9fc3e515367e310f5964af5a065799dc6ea9eb Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 2 Dec 2024 15:42:43 +0100 Subject: [PATCH 16/17] fix: corrects offscreen scene creation --- mujoco_ros/src/mujoco_env.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_ros/src/mujoco_env.cpp b/mujoco_ros/src/mujoco_env.cpp index 645c9393..40cfd0fd 100644 --- a/mujoco_ros/src/mujoco_env.cpp +++ b/mujoco_ros/src/mujoco_env.cpp @@ -131,7 +131,7 @@ MujocoEnv::MujocoEnv(const std::string &admin_hash /* = std::string()*/) } nh_->param("render_offscreen", settings_.render_offscreen, true); - if (!settings_.render_offscreen) { + if (settings_.render_offscreen) { mjv_makeScene(nullptr, &offscreen_.scn, Viewer::kMaxGeom); } From b8ce3da8e24f70fa92620c0799dcaaf878ae1821 Mon Sep 17 00:00:00 2001 From: David Leins Date: Mon, 2 Dec 2024 15:43:47 +0100 Subject: [PATCH 17/17] fix: adds bool to signal OSMesa cleanup necessity --- mujoco_ros/include/mujoco_ros/mujoco_env.h | 1 + mujoco_ros/src/offscreen_rendering.cpp | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/mujoco_ros/include/mujoco_ros/mujoco_env.h b/mujoco_ros/include/mujoco_ros/mujoco_env.h index 0e86db90..9fed622a 100644 --- a/mujoco_ros/include/mujoco_ros/mujoco_env.h +++ b/mujoco_ros/include/mujoco_ros/mujoco_env.h @@ -128,6 +128,7 @@ struct OffscreenRenderContext { OSMesaContext ctx; unsigned char buffer[10000000]; // TODO: size necessary or resize later? + bool initialized = false; } osmesa; #endif mjrContext con = {}; diff --git a/mujoco_ros/src/offscreen_rendering.cpp b/mujoco_ros/src/offscreen_rendering.cpp index 98beb73e..f1ed9bce 100644 --- a/mujoco_ros/src/offscreen_rendering.cpp +++ b/mujoco_ros/src/offscreen_rendering.cpp @@ -77,6 +77,9 @@ OffscreenRenderContext::~OffscreenRenderContext() eglTerminate(display); } #elif RENDER_BACKEND == USE_OSMESA + if (!osmesa.initialized) { + return; + } ROS_DEBUG("Freeing OSMESA offscreen context"); mjr_defaultContext(&con); mjr_freeContext(&con); @@ -256,6 +259,7 @@ bool MujocoEnv::InitGL() return false; } ROS_DEBUG("OSMesa initialized"); + offscreen_.osmesa.initialized = true; return true; } // InitGL #endif