SensorPublisher.h

Go to the documentation of this file.

#pragma once
 
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
 
#include <mujoco_ros_utils/msg/scalar_stamped.hpp>
#include <string>
 
namespace MujocoRosUtils
{
 
class SensorPublisher
{
public:
  typedef enum MessageType_
  {
    MsgScalar = 0,
 
    MsgPoint,
 
    MsgVector3,
 
    MsgQuaternion
  } MessageType;
 
public:
  static void RegisterPlugin();
 
  static SensorPublisher * Create(const mjModel * m, mjData * d, int plugin_id);
 
public:
  SensorPublisher(SensorPublisher &&) = default;
 
  void reset(const mjModel * m, int plugin_id);
 
  void compute(const mjModel * m, mjData * d, int plugin_id);
 
protected:
  SensorPublisher(const mjModel * m,
                  mjData * d,
                  int sensor_id,
                  MessageType msg_type,
                  const std::string & frame_id,
                  const std::string & topic_name,
                  mjtNum publish_rate);
 
protected:
  int sensor_id_ = -1;
 
  MessageType msg_type_;
 
  std::string frame_id_;
 
  std::string topic_name_;
 
  rclcpp::Node::SharedPtr nh_;
 
  rclcpp::PublisherBase::SharedPtr pub_;
 
  int publish_skip_ = 0;
 
  int sim_cnt_ = 0;
};
 
} // namespace MujocoRosUtils