Merge branch 'main' into main

Author: pucciland95

.github/dependabot.yml

@@ -0,0 +1,6 @@
+version: 2
+updates:
+  - package-ecosystem: "github-actions"
+    directory: "/"
+    schedule:
+      interval: "weekly"

.github/workflows/ci.yml

@@ -0,0 +1,49 @@
+name: CI
+on:
+  workflow_dispatch:
+  pull_request:
+  merge_group:
+  push:
+    branches:
+      - main
+
+env:
+  ROS_DISTRO: humble
+
+jobs:
+  pre-commit:
+    runs-on: ubuntu-22.04
+    steps:
+    - name: Checkout code from current branch
+      uses: actions/checkout@v4
+    - name: Setup ROS
+      uses: ros-tooling/setup-ros@v0.7
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo rosdep update --rosdistro ${{ env.ROS_DISTRO }}
+        sudo rosdep install --from-paths . --ignore-src -y -t test --rosdistro ${{ env.ROS_DISTRO }}
+    - name: Run pre-commit
+      run: |
+        pip3 install pre-commit
+        source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash
+        pre-commit run --all-files
+
+  build_and_test:
+    needs: pre-commit
+    runs-on: ubuntu-latest
+    steps:
+    - name: Checkout code from current branch
+      uses: actions/checkout@v4
+    - name: Set up Docker Buildx
+      uses: docker/setup-buildx-action@v3
+    - name: Build in Docker container
+      uses: docker/build-push-action@v6
+      with:
+        context: .
+        file: docker/Dockerfile
+        push: false
+        load: true
+        tags: mujoco_ros2_control:latest
+    - name: Test in Docker container
+      run: docker run --entrypoint "/bin/bash" --rm mujoco_ros2_control:latest -c "colcon test && colcon test-result --verbose"

.github/workflows/stale.yml

@@ -0,0 +1,20 @@
+name: 'Close stale issues and PR'
+on:
+  schedule:
+    - cron: '30 1 * * *'
+
+jobs:
+  stale:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/stale@v9
+        with:
+          stale-issue-message: 'This issue is stale because it has been open 60 days with no activity. Remove stale label or comment or this will be closed in 30 days.'
+          stale-pr-message: 'This PR is stale because it has been open 60 days with no activity. Please ensure the PR is still relevant. If not, please close the PR.'
+          close-issue-message: 'This issue was closed because it has been stalled for 30 days with no activity.'
+          days-before-stale: 60
+          days-before-close: 30
+          days-before-pr-close: -1
+          stale-issue-label: 'stale'
+          stale-pr-label: 'stale'
+          exempt-issue-labels: 'question,bug,documentation,enhancement,good first issue,help wanted'

doc/index.rst

@@ -33,17 +33,26 @@ Use ``mujoco_ros2_control/MujocoSystem`` for plugin
     </joint>
   </ros2_control>
 
-Convert URDF model to xml
+Convert URDF model to XML
 --------------------------
-You need to convert the URDF model to a MJCF XML file.
+URDF models must be converted to `MJCF XML <https://mujoco.readthedocs.io/en/latest/modeling.html>`_ files.
 Make sure to use the same name for the link and joint, which are mapped to the body and joint in Mujoco.
-You need to specify <limit> which is mapped to ``range`` in MJCF. For now, there is no way to specify velocity or acceleration limit.
+You need to specify <limit> which is mapped to ``range`` in MJCF.
+For now, there is no way to specify velocity or acceleration limit.
 
-For force torque sensor, you need to map the sensor to a force sensor and a torque sensor in MJCF since there is no combined force torque sensor in MuJoCo.
-The name of each sensor should be ``sensor_name`` + ``_force`` and ``sensor_name`` + ``_torque``.
-For example, if you have a force torque sensor called ``my_sensor``, you need to create ``my_sensor_force`` and ``my_sensor_torque`` in MJCF.
+For force torque sensors, there is no combined force torque sensor in MuJoCo.
+Therefore in ROS, a single force torque sensor must be mapped to both a force sensor and a torque sensor in MJCF.
+The name of each sensor must be ``sensor_name`` + ``_force`` and ``sensor_name`` + ``_torque``.
+For example, the mapped names for a ROS force torque sensor ``my_sensor`` would be ``my_sensor_force`` and ``my_sensor_torque`` in MJCF.
 
-Check ``mujoco_ros2_control_demos/mujoco_models`` for examples.
+The drivers additionally support simulated RGB-D cameras for publishing simulated color images and depth maps.
+Cameras must be given a name and be attached to a joint called ``<name>_optical_frame``.
+The camera_info, color, and depth images will be published to topics called ``<name>/camera_info``,
+``<name>/color``, and ``<name>/depth``, repectively.
+Also note that MuJuCo's conventions for cameras are different than ROS's, and which must be accounted for.
+An overview is available through the "camera" demo.
+
+For additional information refer to the ``mujoco_ros2_control_demos/mujoco_models`` for examples.
 
 Specify the location of Mujoco models and the controller configuration file
 ----------------------------------------------------------------------------

mujoco_ros2_control/CMakeLists.txt

@@ -71,7 +71,12 @@ install(TARGETS
 )
 
 # TODO: make it simple
-add_executable(mujoco_ros2_control src/mujoco_ros2_control_node.cpp src/mujoco_rendering.cpp src/mujoco_ros2_control.cpp)
+add_executable(mujoco_ros2_control
+  src/mujoco_ros2_control_node.cpp
+  src/mujoco_rendering.cpp
+  src/mujoco_ros2_control.cpp
+  src/mujoco_cameras.cpp
+)
 ament_target_dependencies(mujoco_ros2_control ${THIS_PACKAGE_DEPENDS})
 target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} glfw)
 target_include_directories(mujoco_ros2_control

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_cameras.hpp

@@ -0,0 +1,84 @@
+// Copyright (c) 2025 Erik Holum
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "GLFW/glfw3.h"
+#include "mujoco/mujoco.h"
+#include "rclcpp/rclcpp.hpp"
+
+#include "rclcpp/node.hpp"
+#include "rclcpp/publisher.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "sensor_msgs/msg/image.hpp"
+
+namespace mujoco_ros2_control
+{
+
+struct CameraData
+{
+  mjvCamera mjv_cam;
+  mjrRect viewport;
+
+  std::string name;
+  std::string frame_name;
+  uint32_t width;
+  uint32_t height;
+
+  std::vector<uint8_t> image_buffer;
+  std::vector<float> depth_buffer;
+  std::vector<float> depth_buffer_flipped;
+
+  sensor_msgs::msg::Image image;
+  sensor_msgs::msg::Image depth_image;
+  sensor_msgs::msg::CameraInfo camera_info;
+
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth_image_pub;
+  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_pub;
+};
+
+class MujocoCameras
+{
+public:
+  explicit MujocoCameras(rclcpp::Node::SharedPtr &node);
+
+  void init(mjModel *mujoco_model);
+  void update(mjModel *mujoco_model, mjData *mujoco_data);
+  void close();
+
+private:
+  void register_cameras(const mjModel *mujoco_model);
+
+  rclcpp::Node::SharedPtr node_;
+
+  // Rendering options for the cameras, currently hard coded to defaults
+  mjvOption mjv_opt_;
+  mjvScene mjv_scn_;
+  mjrContext mjr_con_;
+
+  // Containers for camera data and ROS constructs
+  std::vector<CameraData> cameras_;
+};
+
+}  // namespace mujoco_ros2_control

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp

@@ -21,12 +21,18 @@
 #ifndef MUJOCO_ROS2_CONTROL__MUJOCO_RENDERING_HPP_
 #define MUJOCO_ROS2_CONTROL__MUJOCO_RENDERING_HPP_
 
+#include <string>
+#include <vector>
+
 #include "GLFW/glfw3.h"
 #include "mujoco/mujoco.h"
 #include "rclcpp/rclcpp.hpp"
 
+#include "rclcpp/node.hpp"
+
 namespace mujoco_ros2_control
 {
+
 class MujocoRendering
 {
 public:
@@ -45,21 +51,26 @@ class MujocoRendering
   static void mouse_button_callback(GLFWwindow *window, int button, int act, int mods);
   static void mouse_move_callback(GLFWwindow *window, double xpos, double ypos);
   static void scroll_callback(GLFWwindow *window, double xoffset, double yoffset);
+
   void keyboard_callback_impl(GLFWwindow *window, int key, int scancode, int act, int mods);
   void mouse_button_callback_impl(GLFWwindow *window, int button, int act, int mods);
   void mouse_move_callback_impl(GLFWwindow *window, double xpos, double ypos);
   void scroll_callback_impl(GLFWwindow *window, double xoffset, double yoffset);
 
   static MujocoRendering *instance_;
+
   mjModel *mj_model_;
   mjData *mj_data_;
+
+  // Window and primary camera for the simulation's viewer
+  GLFWwindow *window_;
   mjvCamera mjv_cam_;
+
+  // Options for the rendering context and scene, all of these are hard coded to defaults.
   mjvOption mjv_opt_;
   mjvScene mjv_scn_;
   mjrContext mjr_con_;
 
-  GLFWwindow *window_;
-
   bool button_left_;
   bool button_middle_;
   bool button_right_;