ImagePublisher.h

Go to the documentation of this file.

#pragma once
 
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>
 
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjrender.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
 
#include <GLFW/glfw3.h>
#include <string>
 
namespace MujocoRosUtils
{
 
class ImagePublisher
{
public:
  static void RegisterPlugin();
 
  static ImagePublisher * Create(const mjModel * m, mjData * d, int plugin_id);
 
public:
  ImagePublisher(ImagePublisher &&) = default;
 
  void reset(const mjModel * m, int plugin_id);
 
  void compute(const mjModel * m, mjData * d, int plugin_id);
 
  void free();
 
protected:
  ImagePublisher(const mjModel * m,
                 mjData * d,
                 int sensor_id,
                 const std::string & frame_id,
                 std::string color_topic_name,
                 std::string depth_topic_name,
                 std::string info_topic_name,
                 int height,
                 int width,
                 mjtNum publish_rate);
 
protected:
  int sensor_id_ = -1;
 
  int camera_id_ = -1;
 
  std::string frame_id_;
 
  int publish_skip_ = 0;
 
  int sim_cnt_ = 0;
 
  unsigned char * color_buffer_;
  float * depth_buffer_;
  unsigned char * color_buffer_flipped_;
  float * depth_buffer_flipped_;
 
  mjvScene scene_;
  mjvCamera camera_;
  mjvOption option_;
  mjrContext context_;
  mjrRect viewport_;
  GLFWwindow * window_;
 
  rclcpp::Node::SharedPtr nh_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr color_pub_;
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr depth_pub_;
  rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr info_pub_;
};
 
} // namespace MujocoRosUtils