RigidBodyTransform transform = new RigidBodyTransform(rotation, translation); tfPublisher.publish(transform, timeStamp, "fiducial" + id, "camera");
Transform3d transform = new Transform3d(rotation, translation); tfPublisher.publish(transform, timeStamp, "fiducial" + id, "camera");
public void publish(RigidBodyTransform transform3d, long timeStamp, String parentFrame, String childFrame) { TransformStamped transformStamped = topicMessageFactory.newFromType(TransformStamped._TYPE); Transform transform = getRosTransform(transform3d); transformStamped.setTransform(transform); transformStamped.setChildFrameId(childFrame); Header header = transformStamped.getHeader(); header.setStamp(Time.fromNano(timeStamp)); header.setFrameId(parentFrame); header.setSeq(seq++); transformStamped.setHeader(header); TFMessage message = getMessage(); List<TransformStamped> tfs = new ArrayList<TransformStamped>(); tfs.add(transformStamped); message.setTransforms(tfs); publish(message); } }
public void publish(RigidBodyTransform transform3d, long timeStamp, String parentFrame, String childFrame) { TransformStamped transformStamped = topicMessageFactory.newFromType(TransformStamped._TYPE); Transform transform = getRosTransform(transform3d); transformStamped.setTransform(transform); transformStamped.setChildFrameId(childFrame); Header header = transformStamped.getHeader(); header.setStamp(Time.fromNano(timeStamp)); header.setFrameId(parentFrame); header.setSeq(seq++); transformStamped.setHeader(header); TFMessage message = getMessage(); List<TransformStamped> tfs = new ArrayList<TransformStamped>(); tfs.add(transformStamped); message.setTransforms(tfs); publish(message); } }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { Transform3d tmpTransform = new Transform3d(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quat4d(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { RigidBodyTransform tmpTransform = new RigidBodyTransform(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quaternion(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }
@Override public void updateRigidbodies(ArrayList<MocapRigidBody> listOfRigidbodies) { if(!mainNode.isStarted()) return; for (MocapRigidBody rigidBody : listOfRigidbodies) { Transform3d tmpTransform = new Transform3d(); tmpTransform.setTranslation(rigidBody.xPosition,rigidBody.yPosition,rigidBody.zPosition); tmpTransform.setRotation(new Quat4d(rigidBody.qx, rigidBody.qy, rigidBody.qz, rigidBody.qw)); tfPublisher.publish(tmpTransform, mainNode.getCurrentTime().totalNsecs(), "/mocap_world", "mocap/rigidBody"+rigidBody.getId()); } //System.out.println("Update rate: " + frequencyCalculator.determineCallFrequency() + " Hz"); }