From 2e3428c66dccefbf61ed9dc0effdaeff8e22acc0 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Fri, 19 Dec 2025 12:34:35 -0800 Subject: [PATCH 1/3] Enable colcon rolling build in ROS 2 workspace --- CMakeLists.txt | 88 +++++++++++++++++++++++++++++++++++++++++++++++++- README.md | 45 ++++++++++++++++++++++++++ package.xml | 1 + 3 files changed, 133 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8f5285d..d1b4197 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,6 +2,49 @@ cmake_minimum_required(VERSION 3.16) project(mujoco_ros2_control) +# Find mold linker (required dependency, Linux only) +# mold is Linux-specific and not available on macOS/Windows +if(CMAKE_SYSTEM_NAME STREQUAL "Linux") + find_program(MOLD_LINKER + NAMES mold + PATHS /usr/bin /usr/local/bin /opt/conda/bin ${CONDA_PREFIX}/bin + NO_DEFAULT_PATH + ) + if(NOT MOLD_LINKER) + find_program(MOLD_LINKER mold) + endif() + + if(NOT MOLD_LINKER) + message(FATAL_ERROR "mold linker not found. Please install mold: sudo apt-get install mold") + endif() + message(STATUS "Found mold linker: ${MOLD_LINKER}") + + # Set mold as the linker if not already set via environment + if(NOT DEFINED ENV{CXXFLAGS} OR NOT "$ENV{CXXFLAGS}" MATCHES "-fuse-ld=mold") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=mold") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -fuse-ld=mold") + message(STATUS "Using mold linker for faster builds") + endif() +else() + message(STATUS "mold linker skipped (not available on ${CMAKE_SYSTEM_NAME})") +endif() + +# Find sccache (required dependency for compiler caching) +# Try multiple methods: standard PATH, common install locations +find_program(SCCACHE_PROGRAM + NAMES sccache + PATHS /usr/bin /usr/local/bin /opt/conda/bin ${CONDA_PREFIX}/bin + NO_DEFAULT_PATH +) +if(NOT SCCACHE_PROGRAM) + find_program(SCCACHE_PROGRAM sccache) +endif() + +if(NOT SCCACHE_PROGRAM) + message(FATAL_ERROR "sccache not found. Please install sccache: sudo apt-get install sccache (or via cargo: cargo install sccache)") +endif() +message(STATUS "Found sccache: ${SCCACHE_PROGRAM}") + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() @@ -10,7 +53,50 @@ endif() find_package(ament_cmake REQUIRED) find_package(controller_manager REQUIRED) find_package(Eigen3 REQUIRED) -find_package(glfw3 REQUIRED) + +# Find GLFW +# Standard approach: use pkg-config for system packages (libglfw3-dev provides .pc file) +# Fallback to CMake config for conda/pixi environments, then manual find +find_package(PkgConfig QUIET) +if(PkgConfig_FOUND) + pkg_check_modules(GLFW3 glfw3) +endif() + +if(GLFW3_FOUND) + # Create glfw target from pkg-config (standard for system packages) + add_library(glfw INTERFACE) + target_include_directories(glfw INTERFACE ${GLFW3_INCLUDE_DIRS}) + target_link_libraries(glfw INTERFACE ${GLFW3_LIBRARIES}) + target_compile_options(glfw INTERFACE ${GLFW3_CFLAGS_OTHER}) +else() + # Fallback: try CMake config (for conda/pixi environments) + find_package(glfw3 QUIET) + if(glfw3_FOUND) + if(TARGET glfw3::glfw3 AND NOT TARGET glfw) + add_library(glfw ALIAS glfw3::glfw3) + elseif(TARGET glfw3 AND NOT TARGET glfw) + add_library(glfw ALIAS glfw3) + endif() + else() + # Last resort: manual find (shouldn't be needed if libglfw3-dev is installed) + find_library(GLFW3_LIBRARY + NAMES glfw glfw3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + find_path(GLFW3_INCLUDE_DIR + NAMES GLFW/glfw3.h + PATHS /usr/include /usr/local/include + ) + if(GLFW3_LIBRARY AND GLFW3_INCLUDE_DIR) + add_library(glfw INTERFACE) + target_include_directories(glfw INTERFACE ${GLFW3_INCLUDE_DIR}) + target_link_libraries(glfw INTERFACE ${GLFW3_LIBRARY}) + else() + message(FATAL_ERROR "GLFW3 not found. Please install libglfw3-dev: sudo apt-get install libglfw3-dev") + endif() + endif() +endif() + find_package(control_toolbox REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) diff --git a/README.md b/README.md index c0bc842..38cec30 100644 --- a/README.md +++ b/README.md @@ -454,3 +454,48 @@ ros2 topic pub /position_controller/commands std_msgs/msg/Float64MultiArray "dat > UI panels can be toggled with `Tab` or `Shift+Tab`. > All standard MuJoCo keyboard shortcuts are available. > To see a short list, press `F1`. + +## Development Workflow within ROS 2 Workspace + +This package can be built in a standard ROS 2 workspace on Linux/Ubuntu. This workflow is recommended for integrating with existing ROS 2 workspaces or for developers working with rolling builds from `ros2_control` or `ros2_controllers` projects. For isolated development environments, consider using the [Docker Development Workflow](#docker-development-workflow) or [Pixi Development Workflow](#pixi-development-workflow) instead. + +> [!WARNING] +> Special care is needed when building in a ROS 2 workspace because ROS 2 packages from system installations or rolling builds may not be compatible with the versions pinned in `pixi.lock`. Version mismatches can cause ABI incompatibilities leading to segmentation faults (SIGSEGV) during runtime, such as `ros2_control_node` crashing with exit code -11 during hardware initialization. Ensure that your ROS 2 workspace uses compatible package versions to avoid build conflicts and runtime crashes. See [issue #30](https://github.com/ros-controls/mujoco_ros2_control/issues/30) for more details. + +### Dependencies + +Install required system dependencies: + +```bash +# Install ROS 2 dependencies via rosdep +rosdep update --rosdistro $ROS_DISTRO +rosdep install --from-paths src --ignore-src -r -y + +# Install additional build dependencies +sudo apt-get install libglfw3-dev sccache mold +``` + +**Note:** `mold` is a Linux-only linker optimization. On macOS/Windows, it will be automatically skipped during the build. + +### Build + +From your ROS 2 workspace root: + +```bash +# Source ROS 2 installation +source /opt/ros/$ROS_DISTRO/setup.bash + +# Build the package +colcon build --symlink-install --packages-select mujoco_ros2_control + +# Source the workspace +source install/setup.bash +``` + +The build system will: +- Automatically download MuJoCo 3.3.4 if not found locally +- Use `pkg-config` to find GLFW (system package) or fall back to CMake config (conda/pixi environments) +- Require `sccache` for compiler caching (cross-platform) +- Require `mold` linker on Linux for faster linking (skipped on other platforms) + +If any required dependency is missing, CMake will provide clear error messages with installation instructions. diff --git a/package.xml b/package.xml index ed2f1f5..5db3e69 100644 --- a/package.xml +++ b/package.xml @@ -13,6 +13,7 @@ controller_manager hardware_interface libglfw3-dev + sccache pluginlib rclcpp rclcpp_lifecycle From 12a961ccb0ab1a45cde5b03e6afc757da91a00f7 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Sat, 20 Dec 2025 15:38:01 -0800 Subject: [PATCH 2/3] Fix glfw linking error and shutdown race condition --- CMakeLists.txt | 29 ++++++++++--- src/mujoco_cameras.cpp | 11 +++++ src/mujoco_system_interface.cpp | 74 +++++++++++++++++++++++++++++---- 3 files changed, 101 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d1b4197..8d75ac1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,10 +64,27 @@ endif() if(GLFW3_FOUND) # Create glfw target from pkg-config (standard for system packages) - add_library(glfw INTERFACE) - target_include_directories(glfw INTERFACE ${GLFW3_INCLUDE_DIRS}) - target_link_libraries(glfw INTERFACE ${GLFW3_LIBRARIES}) - target_compile_options(glfw INTERFACE ${GLFW3_CFLAGS_OTHER}) + # Use find_library to get actual library path for proper linking + find_library(GLFW3_LIBRARY_FILE + NAMES glfw glfw3 + PATHS ${GLFW3_LIBRARY_DIRS} /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + NO_DEFAULT_PATH + ) + if(NOT GLFW3_LIBRARY_FILE) + find_library(GLFW3_LIBRARY_FILE NAMES glfw glfw3) + endif() + if(GLFW3_LIBRARY_FILE) + add_library(glfw INTERFACE) + target_include_directories(glfw INTERFACE ${GLFW3_INCLUDE_DIRS}) + target_link_libraries(glfw INTERFACE ${GLFW3_LIBRARY_FILE}) + target_compile_options(glfw INTERFACE ${GLFW3_CFLAGS_OTHER}) + else() + # Fallback to pkg-config libraries if find_library fails + add_library(glfw INTERFACE) + target_include_directories(glfw INTERFACE ${GLFW3_INCLUDE_DIRS}) + target_link_libraries(glfw INTERFACE ${GLFW3_LIBRARIES}) + target_compile_options(glfw INTERFACE ${GLFW3_CFLAGS_OTHER}) + endif() else() # Fallback: try CMake config (for conda/pixi environments) find_package(glfw3 QUIET) @@ -215,7 +232,7 @@ target_include_directories( platform_ui_adapter PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR} $ ) -target_link_libraries(platform_ui_adapter PUBLIC ${MUJOCO_LIB}) +target_link_libraries(platform_ui_adapter PUBLIC ${MUJOCO_LIB} glfw) target_compile_options(platform_ui_adapter PRIVATE ${MUJOCO_SILENCE_COMPILER_WARNINGS}) add_library(mujoco::platform_ui_adapter ALIAS platform_ui_adapter) @@ -231,7 +248,7 @@ target_sources( PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.cc ${MUJOCO_SIMULATE_DIR}/array_safety.h ) target_include_directories(libsimulate PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR}) -target_link_libraries(libsimulate PUBLIC lodepng mujoco::platform_ui_adapter ${MUJOCO_LIB}) +target_link_libraries(libsimulate PUBLIC lodepng mujoco::platform_ui_adapter ${MUJOCO_LIB} glfw) target_compile_options(libsimulate PRIVATE ${MUJOCO_SILENCE_COMPILER_WARNINGS}) # Mujoco ROS 2 control system interface (this package) diff --git a/src/mujoco_cameras.cpp b/src/mujoco_cameras.cpp index 50292db..5dec1bc 100644 --- a/src/mujoco_cameras.cpp +++ b/src/mujoco_cameras.cpp @@ -198,12 +198,23 @@ void MujocoCameras::update_loop() void MujocoCameras::update() { + // Safety check: ensure model and data are still valid + if (!mj_model_ || !mj_data_ || !mj_camera_data_) + { + return; + } + // Rendering is done offscreen mjr_setBuffer(mjFB_OFFSCREEN, &mjr_con_); // Step 1: Lock the sim and copy data for use in all camera rendering. { std::unique_lock lock(*sim_mutex_); + // Double-check after acquiring lock (model/data might be deleted during shutdown) + if (!mj_model_ || !mj_data_ || !mj_camera_data_) + { + return; + } mjv_copyData(mj_camera_data_, mj_model_, mj_data_); } diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 5fb61eb..d1fef85 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -463,26 +463,29 @@ MujocoSystemInterface::MujocoSystemInterface() = default; MujocoSystemInterface::~MujocoSystemInterface() { - // Stop camera rendering loop + // Stop camera rendering loop (if not already stopped in on_deactivate) if (cameras_) { cameras_->close(); } - // Stop lidar sensor loop + // Stop lidar sensor loop (if not already stopped in on_deactivate) if (lidar_sensors_) { lidar_sensors_->close(); } - // Stop ROS + // Stop ROS executor if (executor_) { executor_->cancel(); - executor_thread_.join(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } } - // If sim_ is created and running, clean shut it down + // If sim_ is created and running, clean shut it down (if not already stopped in on_deactivate) if (sim_) { sim_->exitrequest.store(true); @@ -499,17 +502,21 @@ MujocoSystemInterface::~MujocoSystemInterface() } // Cleanup data and the model, if they haven't been + // Only cleanup after all threads are stopped to avoid race conditions if (mj_data_) { mj_deleteData(mj_data_); + mj_data_ = nullptr; } if (mj_data_control_) { mj_deleteData(mj_data_control_); + mj_data_control_ = nullptr; } if (mj_model_) { mj_deleteModel(mj_model_); + mj_model_ = nullptr; } } @@ -946,7 +953,42 @@ MujocoSystemInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_s { RCLCPP_INFO(get_logger(), "Deactivating MuJoCo hardware interface and shutting down Simulate..."); - // TODO: Should we shut mujoco things down here or in the destructor? + // Stop ROS executor first (it may be publishing clock or accessing resources) + if (executor_) + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + + // Stop camera and lidar rendering loops before ROS context becomes invalid + if (cameras_) + { + cameras_->close(); + } + + if (lidar_sensors_) + { + lidar_sensors_->close(); + } + + // Stop physics and UI threads before ROS context becomes invalid + if (sim_) + { + sim_->exitrequest.store(true); + sim_->run = false; + + if (physics_thread_.joinable()) + { + physics_thread_.join(); + } + if (ui_thread_.joinable()) + { + ui_thread_.join(); + } + } return hardware_interface::CallbackReturn::SUCCESS; } @@ -1633,7 +1675,7 @@ void MujocoSystemInterface::PhysicsLoop() mjtNum syncSim = 0; // run until asked to exit - while (!sim_->exitrequest.load()) + while (sim_ && !sim_->exitrequest.load()) { // sleep for 1 ms or yield, to let main thread run // yield results in busy wait - which has better timing but kills battery life @@ -1647,9 +1689,21 @@ void MujocoSystemInterface::PhysicsLoop() } { + // Safety check: ensure sim_ and sim_mutex_ are still valid + if (!sim_ || !sim_mutex_) + { + break; + } + // lock the sim mutex during the update const std::unique_lock lock(*sim_mutex_); + // Double-check after acquiring lock (sim_ might be deleted during shutdown) + if (!sim_ || !mj_model_ || !mj_data_) + { + break; + } + // run only if model is present if (mj_model_) { @@ -1781,6 +1835,12 @@ void MujocoSystemInterface::PhysicsLoop() void MujocoSystemInterface::publish_clock() { + // Safety check: ensure mj_data_ is still valid (may be deleted during shutdown) + if (!mj_data_ || !clock_realtime_publisher_) + { + return; + } + auto sim_time = mj_data_->time; int32_t sim_time_sec = static_cast(std::floor(sim_time)); uint32_t sim_time_nanosec = static_cast((sim_time - sim_time_sec) * 1e9); From 539685c7f86c8656ceb2c6cde80c7867c0a6b1b3 Mon Sep 17 00:00:00 2001 From: Julia Jia Date: Sun, 21 Dec 2025 09:15:15 -0800 Subject: [PATCH 3/3] Remove non build related local changes --- README.md | 1 - src/mujoco_cameras.cpp | 11 ----- src/mujoco_system_interface.cpp | 74 ++++----------------------------- 3 files changed, 7 insertions(+), 79 deletions(-) diff --git a/README.md b/README.md index fa92a89..6ecd22b 100644 --- a/README.md +++ b/README.md @@ -394,4 +394,3 @@ ros2 topic pub /position_controller/commands std_msgs/msg/Float64MultiArray "dat ## Development More information is provided in the [developers guide](./docs/DEVELOPMENT.md) document. - diff --git a/src/mujoco_cameras.cpp b/src/mujoco_cameras.cpp index 5dec1bc..50292db 100644 --- a/src/mujoco_cameras.cpp +++ b/src/mujoco_cameras.cpp @@ -198,23 +198,12 @@ void MujocoCameras::update_loop() void MujocoCameras::update() { - // Safety check: ensure model and data are still valid - if (!mj_model_ || !mj_data_ || !mj_camera_data_) - { - return; - } - // Rendering is done offscreen mjr_setBuffer(mjFB_OFFSCREEN, &mjr_con_); // Step 1: Lock the sim and copy data for use in all camera rendering. { std::unique_lock lock(*sim_mutex_); - // Double-check after acquiring lock (model/data might be deleted during shutdown) - if (!mj_model_ || !mj_data_ || !mj_camera_data_) - { - return; - } mjv_copyData(mj_camera_data_, mj_model_, mj_data_); } diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index d1fef85..5fb61eb 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -463,29 +463,26 @@ MujocoSystemInterface::MujocoSystemInterface() = default; MujocoSystemInterface::~MujocoSystemInterface() { - // Stop camera rendering loop (if not already stopped in on_deactivate) + // Stop camera rendering loop if (cameras_) { cameras_->close(); } - // Stop lidar sensor loop (if not already stopped in on_deactivate) + // Stop lidar sensor loop if (lidar_sensors_) { lidar_sensors_->close(); } - // Stop ROS executor + // Stop ROS if (executor_) { executor_->cancel(); - if (executor_thread_.joinable()) - { - executor_thread_.join(); - } + executor_thread_.join(); } - // If sim_ is created and running, clean shut it down (if not already stopped in on_deactivate) + // If sim_ is created and running, clean shut it down if (sim_) { sim_->exitrequest.store(true); @@ -502,21 +499,17 @@ MujocoSystemInterface::~MujocoSystemInterface() } // Cleanup data and the model, if they haven't been - // Only cleanup after all threads are stopped to avoid race conditions if (mj_data_) { mj_deleteData(mj_data_); - mj_data_ = nullptr; } if (mj_data_control_) { mj_deleteData(mj_data_control_); - mj_data_control_ = nullptr; } if (mj_model_) { mj_deleteModel(mj_model_); - mj_model_ = nullptr; } } @@ -953,42 +946,7 @@ MujocoSystemInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_s { RCLCPP_INFO(get_logger(), "Deactivating MuJoCo hardware interface and shutting down Simulate..."); - // Stop ROS executor first (it may be publishing clock or accessing resources) - if (executor_) - { - executor_->cancel(); - if (executor_thread_.joinable()) - { - executor_thread_.join(); - } - } - - // Stop camera and lidar rendering loops before ROS context becomes invalid - if (cameras_) - { - cameras_->close(); - } - - if (lidar_sensors_) - { - lidar_sensors_->close(); - } - - // Stop physics and UI threads before ROS context becomes invalid - if (sim_) - { - sim_->exitrequest.store(true); - sim_->run = false; - - if (physics_thread_.joinable()) - { - physics_thread_.join(); - } - if (ui_thread_.joinable()) - { - ui_thread_.join(); - } - } + // TODO: Should we shut mujoco things down here or in the destructor? return hardware_interface::CallbackReturn::SUCCESS; } @@ -1675,7 +1633,7 @@ void MujocoSystemInterface::PhysicsLoop() mjtNum syncSim = 0; // run until asked to exit - while (sim_ && !sim_->exitrequest.load()) + while (!sim_->exitrequest.load()) { // sleep for 1 ms or yield, to let main thread run // yield results in busy wait - which has better timing but kills battery life @@ -1689,21 +1647,9 @@ void MujocoSystemInterface::PhysicsLoop() } { - // Safety check: ensure sim_ and sim_mutex_ are still valid - if (!sim_ || !sim_mutex_) - { - break; - } - // lock the sim mutex during the update const std::unique_lock lock(*sim_mutex_); - // Double-check after acquiring lock (sim_ might be deleted during shutdown) - if (!sim_ || !mj_model_ || !mj_data_) - { - break; - } - // run only if model is present if (mj_model_) { @@ -1835,12 +1781,6 @@ void MujocoSystemInterface::PhysicsLoop() void MujocoSystemInterface::publish_clock() { - // Safety check: ensure mj_data_ is still valid (may be deleted during shutdown) - if (!mj_data_ || !clock_realtime_publisher_) - { - return; - } - auto sim_time = mj_data_->time; int32_t sim_time_sec = static_cast(std::floor(sim_time)); uint32_t sim_time_nanosec = static_cast((sim_time - sim_time_sec) * 1e9);