mujoco_ros_utils
Public Member Functions | Static Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
MujocoRosUtils::PosePublisher Class Reference

Plugin to publish topics or broadcast TF of pose and velocity of the body. More...

#include <PosePublisher.h>

Public Member Functions

 PosePublisher (PosePublisher &&)=default
 Copy constructor. More...
 
void reset (const mjModel *m, int plugin_id)
 Reset. More...
 
void compute (const mjModel *m, mjData *d, int plugin_id)
 Compute. More...
 

Static Public Member Functions

static void RegisterPlugin ()
 Register plugin. More...
 
static PosePublisherCreate (const mjModel *m, mjData *d, int plugin_id)
 Create an instance. More...
 

Protected Member Functions

 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)
 Constructor. More...
 

Protected Attributes

int sensor_id_ = -1
 Sensor ID. More...
 
int body_id_ = -1
 Body ID. More...
 
std::shared_ptr< ros::NodeHandle > nh_
 ROS node handle. More...
 
ros::Publisher pose_pub_
 ROS publisher for pose. More...
 
ros::Publisher vel_pub_
 ROS publisher for velocity. More...
 
std::shared_ptr< tf2_ros::TransformBroadcaster > tf_br_
 TF broadcaster. More...
 
std::string frame_id_
 Frame ID of topics header or TF parent. More...
 
std::string pose_topic_name_
 Topic name of pose. More...
 
std::string vel_topic_name_
 Topic name of velocity. More...
 
int publish_skip_ = 0
 Iteration interval to skip ROS publish. More...
 
bool output_tf_ = false
 Whether to broadcast TF. More...
 
std::string tf_child_frame_id_
 Child frame ID for TF. More...
 
int sim_cnt_ = 0
 Iteration count of simulation. More...
 

Detailed Description

Plugin to publish topics or broadcast TF of pose and velocity of the body.

Definition at line 17 of file PosePublisher.h.

Constructor & Destructor Documentation

◆ PosePublisher() [1/2]

MujocoRosUtils::PosePublisher::PosePublisher ( PosePublisher &&  )
default

Copy constructor.

◆ PosePublisher() [2/2]

MujocoRosUtils::PosePublisher::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

Constructor.

Parameters
mmodel
ddata
sensor_idsensor ID
frame_idframe ID of topics header or TF parent
pose_topic_nametopic name of pose
vel_topic_nametopic name of velocity
publish_ratepublish rate
output_tfwhether to broadcast TF
tf_child_frame_idchild frame ID for TF

Definition at line 159 of file PosePublisher.cpp.

Member Function Documentation

◆ compute()

void MujocoRosUtils::PosePublisher::compute ( const mjModel *  m,
mjData *  d,
int  plugin_id 
)

Compute.

Parameters
mmodel
ddata
plugin_idplugin ID

Definition at line 215 of file PosePublisher.cpp.

◆ Create()

PosePublisher * MujocoRosUtils::PosePublisher::Create ( const mjModel *  m,
mjData *  d,
int  plugin_id 
)
static

Create an instance.

Parameters
mmodel
ddata
plugin_idplugin ID

Definition at line 73 of file PosePublisher.cpp.

◆ RegisterPlugin()

void MujocoRosUtils::PosePublisher::RegisterPlugin ( )
static

Register plugin.

Definition at line 14 of file PosePublisher.cpp.

◆ reset()

void MujocoRosUtils::PosePublisher::reset ( const mjModel *  m,
int  plugin_id 
)

Reset.

Parameters
mmodel
plugin_idplugin ID

Definition at line 209 of file PosePublisher.cpp.

Member Data Documentation

◆ body_id_

int MujocoRosUtils::PosePublisher::body_id_ = -1
protected

Body ID.

Definition at line 74 of file PosePublisher.h.

◆ frame_id_

std::string MujocoRosUtils::PosePublisher::frame_id_
protected

Frame ID of topics header or TF parent.

Definition at line 89 of file PosePublisher.h.

◆ nh_

std::shared_ptr<ros::NodeHandle> MujocoRosUtils::PosePublisher::nh_
protected

ROS node handle.

Definition at line 77 of file PosePublisher.h.

◆ output_tf_

bool MujocoRosUtils::PosePublisher::output_tf_ = false
protected

Whether to broadcast TF.

Definition at line 101 of file PosePublisher.h.

◆ pose_pub_

ros::Publisher MujocoRosUtils::PosePublisher::pose_pub_
protected

ROS publisher for pose.

Definition at line 80 of file PosePublisher.h.

◆ pose_topic_name_

std::string MujocoRosUtils::PosePublisher::pose_topic_name_
protected

Topic name of pose.

Definition at line 92 of file PosePublisher.h.

◆ publish_skip_

int MujocoRosUtils::PosePublisher::publish_skip_ = 0
protected

Iteration interval to skip ROS publish.

Definition at line 98 of file PosePublisher.h.

◆ sensor_id_

int MujocoRosUtils::PosePublisher::sensor_id_ = -1
protected

Sensor ID.

Definition at line 71 of file PosePublisher.h.

◆ sim_cnt_

int MujocoRosUtils::PosePublisher::sim_cnt_ = 0
protected

Iteration count of simulation.

Definition at line 107 of file PosePublisher.h.

◆ tf_br_

std::shared_ptr<tf2_ros::TransformBroadcaster> MujocoRosUtils::PosePublisher::tf_br_
protected

TF broadcaster.

Definition at line 86 of file PosePublisher.h.

◆ tf_child_frame_id_

std::string MujocoRosUtils::PosePublisher::tf_child_frame_id_
protected

Child frame ID for TF.

Definition at line 104 of file PosePublisher.h.

◆ vel_pub_

ros::Publisher MujocoRosUtils::PosePublisher::vel_pub_
protected

ROS publisher for velocity.

Definition at line 83 of file PosePublisher.h.

◆ vel_topic_name_

std::string MujocoRosUtils::PosePublisher::vel_topic_name_
protected

Topic name of velocity.

Definition at line 95 of file PosePublisher.h.


The documentation for this class was generated from the following files: