mujoco_cameras.hpp

 
#pragma once
 
#include <atomic>
#include <functional>
#include <mutex>
#include <thread>
#include <vector>
 
#include <EGL/egl.h>
#include <EGL/eglext.h>
#include <GLFW/glfw3.h>
#include <mujoco/mujoco.h>
 
#include <hardware_interface/hardware_info.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.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;
  std::string info_topic;
  std::string image_topic;
  std::string depth_topic;
 
  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:
  using GlfwInitFn = std::function<int()>;
 
  explicit MujocoCameras(rclcpp::Node::SharedPtr node, std::recursive_mutex* sim_mutex, mjData* mujoco_data,
                         mjModel* mujoco_model, double camera_publish_rate);
 
  void init(GlfwInitFn glfw_init_fn = glfwInit);
 
  void close();
 
  void register_cameras(const hardware_interface::HardwareInfo& hardware_info);
 
private:
  void update_loop();
 
  void update();
 
  rclcpp::Node::SharedPtr node_;
 
  // Ensures locked access to simulation data for rendering.
  std::recursive_mutex* sim_mutex_{ nullptr };
 
  mjData* mj_data_;
  mjModel* mj_model_;
  mjData* mj_camera_data_;
 
  // Image publishing rate
  double camera_publish_rate_;
 
  // 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_;
 
  // Camera processing thread
  std::thread rendering_thread_;
  std::atomic_bool publish_images_;
 
  // EGL context for headless rendering (used when GLFW is unavailable)
  EGLDisplay egl_display_{ EGL_NO_DISPLAY };
  EGLContext egl_context_{ EGL_NO_CONTEXT };
  EGLSurface egl_surface_{ EGL_NO_SURFACE };
  bool use_egl_{ false };
 
  bool init_egl_context();
 
  void cleanup_egl_context();
};
 
}  // namespace mujoco_ros2_control