R2RosTreeFk.cpp

Go to the documentation of this file.

#include <robodyn_ros/R2RosTreeFk.hpp>

R2RosTreeFk::R2RosTreeFk(std::string urdf)
{
    treeFk.loadFromFile(urdf);
}

void R2RosTreeFk::setup(std::string inputJointStatesName, std::string outputPoseStatesName)
{
    poseStatesOut = nh.advertise<r2_msgs::PoseState>(outputPoseStatesName, 1);
    jointStatesIn = nh.subscribe(inputJointStatesName, 1, &R2RosTreeFk::jointStatesCallback, this);
}

void R2RosTreeFk::jointStatesCallback(const sensor_msgs::JointState& msg)
{
    treeFk.getPoseState(msg, poseStatesOutMsg);
    poseStatesOut.publish(poseStatesOutMsg);
}