PosePublisher.h

Go to the documentation of this file.

#pragma once
 
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
 
#include <mujoco/mjdata.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjvisualize.h>
 
#include <string>
 
namespace MujocoRosUtils
{
 
class PosePublisher
{
public:
  static void RegisterPlugin();
 
  static PosePublisher * Create(const mjModel * m, mjData * d, int plugin_id);
 
public:
  PosePublisher(PosePublisher &&) = default;
 
  void reset(const mjModel * m, int plugin_id);
 
  void compute(const mjModel * m, mjData * d, int plugin_id);
 
protected:
  PosePublisher(const mjModel * m,
                mjData * d,
                int sensor_id,
                const std::string & frame_id,
                const std::string & pose_topic_name,
                const std::string & vel_topic_name,
                mjtNum publish_rate,
                bool output_tf,
                const std::string & tf_child_frame_id);
 
protected:
  int sensor_id_ = -1;
 
  int body_id_ = -1;
 
  rclcpp::Node::SharedPtr nh_;
 
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
 
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr vel_pub_;
 
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_br_;
 
  std::string frame_id_;
 
  std::string pose_topic_name_;
 
  std::string vel_topic_name_;
 
  int publish_skip_ = 0;
 
  bool output_tf_ = false;
 
  std::string tf_child_frame_id_;
 
  int sim_cnt_ = 0;
};
 
} // namespace MujocoRosUtils