Orientations are not updating

[smihael]

I'm not sure if it's just not implemented or a bug - only the positions and relative positions of the joints are updated and published to tf. Orientations of the joints stay same for the entire runtime of the node.

I noticed from other issues that the project has been discontinued, but it would be nice to hear from you if you had any experience in getting the orientations of the joints (in my understanding this should essentially correspond to the angles between the joints) from the NiTE and what problems did you tackle, so that I could eventually fork this project and develop the missing functionality.

[yodahuang]

I believe it is not implemented, the code only captures position but not orientation of joints, see the code here. I personally do not have a lot of experience dealing with NiTE BTW.

[smihael]

Thanks! I changed

       tf::Quaternion jQ = tf::Quaternion( j.getOrientation().x,  j.getOrientation().y, j.getOrientation().z,  j.getOrientation().w);

        if (j_name != "torso")
        {
            tf::Vector3 rVec3 = tf::Vector3(r.getPosition().x / 1000.0, r.getPosition().y / 1000.0, r.getPosition().z / 1000.0);
            transform.setOrigin(currentVec3 - rVec3);
            tf::Quaternion rQ_conj = tf::Quaternion(-r.getOrientation().x, -r.getOrientation().y,-r.getOrientation().z, r.getOrientation().w);
            transform.setRotation(jQ*rQ_conj);
        } else {
            transform.setOrigin(currentVec3);
            transform.setRotation(jQ);
        }

        if (isnan(transform.getRotation().x()) || isnan(transform.getRotation().y()) ||  isnan(transform.getRotation().z()) || isnan(transform.getRotation().w())) {
           ROS_WARN_STREAM("Got nan as orientation for " << j_name << " in relation to " << r_name << ". Publishing it with identity frame.");
          transform.setRotation(tf::Quaternion::getIdentity());
        }

in the publishJointTF method. A similar solution is used in https://github.com/pal-robotics-forks/openni2_camera/blob/indigo-devel/src/openni2_driver.cpp.

I'd be glad if someone could review this code and merge changes to upstream if it is accepted.

[Harsharma2308]

This might be unrelated, but is there a frequency defined at which the node updates the transforms? So that one can control them.