Skip to content

Commit

Permalink
chg: changed code to 0.8 api in mocap plugin
Browse files Browse the repository at this point in the history
  • Loading branch information
Julian Leichert committed Nov 24, 2023
1 parent ae76eba commit a793e77
Show file tree
Hide file tree
Showing 4 changed files with 17 additions and 17 deletions.
1 change: 0 additions & 1 deletion mujoco_ros_mocap_plugin/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -10,4 +10,3 @@ This can be used in conjunction with a MuJoCo weld constraint to set position an
# Licensing

This work is licensed under the BSD 3-Clause License (see LICENSE).
The original work it is based on was released under a BSD 3-Clause License (see LICENSE-ORIGINAL).
9 changes: 5 additions & 4 deletions mujoco_ros_mocap_plugin/include/mujoco_mocap/mocap_plugin.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,18 +56,19 @@ class MocapPlugin : public mujoco_ros::MujocoPlugin
~MocapPlugin() override = default;

// Overload entry point
virtual bool load(mujoco_ros::mjModelPtr m, mujoco_ros::mjDataPtr d) override;
virtual bool load(const mjModel *m, mjData *d) override;
// Called on reset
virtual void reset() override;

virtual void controlCallback(mujoco_ros::mjModelPtr m, mujoco_ros::mjDataPtr d) override;
virtual void controlCallback(const mjModel *m, mjData *d) override;

private:
void mocapStateCallback(const mujoco_ros_msgs::MocapState::ConstPtr &msg);
bool mocapServiceCallback(mujoco_ros_msgs::SetMocapState::Request &req, mujoco_ros_msgs::SetMocapState::Response &resp);

mujoco_ros::mjModelPtr m;
mujoco_ros::mjDataPtr d;

const mjModel *m_;
mjData *d_;

ros::Subscriber pose_subscriber_;
ros::ServiceServer pose_service_;
Expand Down
2 changes: 1 addition & 1 deletion mujoco_ros_mocap_plugin/package.xml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>mujoco_mocap</name>
<version>1.0.0</version>
<version>0.8.0</version>
<description>The mujoco_mocap plugin package</description>

<url type="repository">https://github.com/ubi-agni/mujoco_mocap_plugin</url>
Expand Down
22 changes: 11 additions & 11 deletions mujoco_ros_mocap_plugin/src/mocap_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,13 +47,13 @@

namespace mujoco_ros::mocap {

bool validateMocapMsg(const mujoco_ros_msgs::MocapState &msg, mujoco_ros::mjModelPtr m) {
bool validateMocapMsg(const mujoco_ros_msgs::MocapState &msg, const mjModel *m) {
for (int idx = 0; idx < msg.pose.size(); idx++) {
if (msg.pose[idx].header.frame_id != "world" && msg.pose[idx].header.frame_id != "") {
ROS_ERROR_STREAM("mocap plugin expects poses in world frame, but got pose in frame " << msg.pose[idx].header.frame_id);
return false;
} else {
int body_id = mj_name2id(m.get(), mjOBJ_BODY, msg.name[idx].c_str());
int body_id = mj_name2id(m, mjOBJ_BODY, msg.name[idx].c_str());
if (body_id == -1) {
ROS_ERROR_STREAM("mocap plugin got pose for unknown body " << msg.name[idx]);
return false;
Expand All @@ -70,14 +70,14 @@ bool validateMocapMsg(const mujoco_ros_msgs::MocapState &msg, mujoco_ros::mjMode
void MocapPlugin::mocapStateCallback(const mujoco_ros_msgs::MocapState::ConstPtr &msg)
{
ROS_DEBUG("Got target poses");
if (!validateMocapMsg(*msg, m)) return;
if (!validateMocapMsg(*msg, m_)) return;
last_mocap_state_ = mujoco_ros_msgs::MocapState(*msg);
}

void MocapPlugin::controlCallback(mujoco_ros::mjModelPtr m, mujoco_ros::mjDataPtr d)
void MocapPlugin::controlCallback(const mjModel *m, mjData *d)
{
for (int idx = 0; idx < last_mocap_state_.pose.size(); idx++) {
int bodyid = mj_name2id(m.get(), mjOBJ_BODY, last_mocap_state_.name[idx].c_str());
int bodyid = mj_name2id(m, mjOBJ_BODY, last_mocap_state_.name[idx].c_str());

if (bodyid == -1) return;
int mocap_data_id = m->body_mocapid[bodyid];
Expand All @@ -104,7 +104,7 @@ void MocapPlugin::controlCallback(mujoco_ros::mjModelPtr m, mujoco_ros::mjDataPt
}

bool MocapPlugin::mocapServiceCallback(mujoco_ros_msgs::SetMocapState::Request &req, mujoco_ros_msgs::SetMocapState::Response &resp) {
if (!validateMocapMsg(req.mocap_state, m)) {
if (!validateMocapMsg(req.mocap_state, m_)) {
resp.success = false;
return true;
}
Expand All @@ -113,19 +113,19 @@ bool MocapPlugin::mocapServiceCallback(mujoco_ros_msgs::SetMocapState::Request &
return true;
}

bool MocapPlugin::load(mujoco_ros::mjModelPtr m, mujoco_ros::mjDataPtr d)
bool MocapPlugin::load(const mjModel *m, mjData *d)
{
// Check that ROS has been initialized
if (!ros::isInitialized()) {
ROS_FATAL("A ROS node for Mujoco has not been initialized, unable to load plugin.");
return false;
}

this->m = m;
this->d = d;
m_ = m;
d_ = d;

pose_subscriber_ = node_handle_->subscribe("mocap_poses", 1, &MocapPlugin::mocapStateCallback, this);
pose_service_ = node_handle_->advertiseService("set_mocap_state", &MocapPlugin::mocapServiceCallback, this);
pose_subscriber_ = node_handle_.subscribe("mocap_poses", 1, &MocapPlugin::mocapStateCallback, this);
pose_service_ = node_handle_.advertiseService("set_mocap_state", &MocapPlugin::mocapServiceCallback, this);
ROS_INFO("Mocap plugin initialized");
return true;
}
Expand Down

0 comments on commit a793e77

Please sign in to comment.