public void publish(long timestamp, RigidBodyTransform transform, Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity, String childFrame, String frameId) { Odometry message = getMessage(); Header header = createHeaderMsg(timestamp); header.setFrameId(frameId); message.setHeader(header); PoseWithCovariance poseWithCovariance = createPoseWithCovarianceMsg(transform); message.setPose(poseWithCovariance); TwistWithCovariance twistWithCovariance = createTwistWithCovariance(linearVelocity, angularVelocity); message.setTwist(twistWithCovariance); message.setChildFrameId(childFrame); publish(message); }
public void publish(long timestamp, RigidBodyTransform transform, Vector3f linearVelocity, Vector3f angularVelocity, String childFrame, String frameId) { Odometry message = getMessage(); Header header = createHeaderMsg(timestamp); header.setFrameId(frameId); message.setHeader(header); PoseWithCovariance poseWithCovariance = createPoseWithCovarianceMsg(transform); message.setPose(poseWithCovariance); TwistWithCovariance twistWithCovariance = createTwistWithCovariance(linearVelocity, angularVelocity); message.setTwist(twistWithCovariance); message.setChildFrameId(childFrame); publish(message); }
pelvisOdometryPublisher.publish(timeStamp, pelvisTransform, robotConfigurationData.getPelvisLinearVelocity(), robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME);
pelvisOdometryPublisher.publish(timeStamp, pelvisTransform, robotConfigurationData.getPelvisLinearVelocity(), robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME);
pelvisOdometryPublisher.publish(timeStamp, pelvisTransform, robotConfigurationData.getPelvisLinearVelocity(), robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME);