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 RosMocapPublisher() { try { mainNode = new RosMainNode(new URI("http://10.7.4.102:11311"), getClass().getSimpleName()); tfPublisher = new RosTf2Publisher(false); } catch (URISyntaxException e) { e.printStackTrace(); } MocapDataClient mocapDataClient = new MocapDataClient(); mocapDataClient.registerRigidBodiesListener(this); }
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");
@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"); }
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 RosMocapPublisher() { try { mainNode = new RosMainNode(new URI("http://10.7.4.102:11311"), getClass().getSimpleName()); tfPublisher = new RosTf2Publisher(false); } catch (URISyntaxException e) { e.printStackTrace(); } MocapDataClient mocapDataClient = new MocapDataClient(); mocapDataClient.registerRigidBodiesListener(this); }
@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"); }
public RosMocapPublisher() { try { mainNode = new RosMainNode(new URI("http://10.7.4.102:11311"), getClass().getSimpleName()); tfPublisher = new RosTf2Publisher(false); } catch (URISyntaxException e) { e.printStackTrace(); } MocapDataClient mocapDataClient = new MocapDataClient(); mocapDataClient.registerRigidBodiesListener(this); }
@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"); }
tfPublisher = new RosTf2Publisher(false);
tfPublisher = new RosTf2Publisher(false);
tfPublisher = new RosTf2Publisher(false); rosMainNode.attachPublisher("/tf", (RosTf2Publisher) tfPublisher);
tfPublisher = new RosTf2Publisher(false); rosMainNode.attachPublisher("/tf", (RosTf2Publisher) tfPublisher);
tfPublisher = new RosTf2Publisher(false); rosMainNode.attachPublisher("/tf", (RosTf2Publisher) tfPublisher);