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); }
private PoseWithCovariance createPoseWithCovarianceMsg(RigidBodyTransform transform) { PoseWithCovariance poseWithCovariance = newMessageFromType(PoseWithCovariance._TYPE); Pose pose = createPoseMsg(transform); poseWithCovariance.setPose(pose); return poseWithCovariance; }
private TwistWithCovariance createTwistWithCovariance(Vector3f linearVelocity, Vector3f angularVelocity) { TwistWithCovariance twistWithCovariance = newMessageFromType(TwistWithCovariance._TYPE); Twist twist = createTwistMsg(linearVelocity, angularVelocity); twistWithCovariance.setTwist(twist); return twistWithCovariance; }
private Twist createTwistMsg(Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity) { Twist twist = newMessageFromType(Twist._TYPE); Vector3 angularVelocityAsRosVector = getVector3(angularVelocity); twist.setAngular(angularVelocityAsRosVector); Vector3 linearVelocityAsRosVector = getVector3(linearVelocity); twist.setLinear(linearVelocityAsRosVector); return twist; }
pelvisOdometryPublisher.publish(timeStamp, pelvisTransform, robotConfigurationData.getPelvisLinearVelocity(), robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME);
this.pelvisOdometryPublisher = new RosOdometryPublisher(latched); this.robotMotionStatusPublisher = new RosStringPublisher(latched); this.robotBehaviorPublisher = new RosInt32Publisher(latched);
private Pose createPoseMsg(RigidBodyTransform transform) { Pose pose = newMessageFromType(geometry_msgs.Pose._TYPE); Point point = newMessageFromType(Point._TYPE); Quaternion rotation = newMessageFromType(Quaternion._TYPE); pose.setPosition(point); pose.setOrientation(rotation); RosTools.packEuclidRigidBodyTransformToGeometry_msgsPose(transform, pose); return pose; } }
pelvisOdometryPublisher.publish(timeStamp, pelvisTransform, robotConfigurationData.getPelvisLinearVelocity(), robotConfigurationData.getPelvisAngularVelocity(), jointMap.getUnsanitizedRootJointInSdf(), WORLD_FRAME);
private Twist createTwistMsg(Vector3f linearVelocity, Vector3f angularVelocity) { Twist twist = newMessageFromType(Twist._TYPE); Vector3 angularVelocityAsRosVector = getVector3(angularVelocity); twist.setAngular(angularVelocityAsRosVector); Vector3 linearVelocityAsRosVector = getVector3(linearVelocity); twist.setLinear(linearVelocityAsRosVector); return twist; }
this.pelvisOdometryPublisher = new RosOdometryPublisher(latched); this.robotMotionStatusPublisher = new RosStringPublisher(latched); this.robotBehaviorPublisher = new RosInt32Publisher(latched);
private Pose createPoseMsg(RigidBodyTransform transform) { Pose pose = newMessageFromType(geometry_msgs.Pose._TYPE); Point point = newMessageFromType(Point._TYPE); Quaternion rotation = newMessageFromType(Quaternion._TYPE); pose.setPosition(point); pose.setOrientation(rotation); RosTools.packRigidBodyTransformToGeometry_msgsPose(transform, pose); return pose; } }
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);
private TwistWithCovariance createTwistWithCovariance(Vector3DReadOnly linearVelocity, Vector3DReadOnly angularVelocity) { TwistWithCovariance twistWithCovariance = newMessageFromType(TwistWithCovariance._TYPE); Twist twist = createTwistMsg(linearVelocity, angularVelocity); twistWithCovariance.setTwist(twist); return twistWithCovariance; }
private PoseWithCovariance createPoseWithCovarianceMsg(RigidBodyTransform transform) { PoseWithCovariance poseWithCovariance = newMessageFromType(PoseWithCovariance._TYPE); Pose pose = createPoseMsg(transform); poseWithCovariance.setPose(pose); return poseWithCovariance; }
this.pelvisOdometryPublisher = new RosOdometryPublisher(latched); this.robotMotionStatusPublisher = new RosStringPublisher(latched); this.robotBehaviorPublisher = new RosInt32Publisher(latched);
private Vector3 getVector3(Vector3DReadOnly angularVelocity) { Vector3 rosVector3 = newMessageFromType(Vector3._TYPE); RosTools.packEuclidTuple3DToGeometry_msgsVector3(angularVelocity, rosVector3); return rosVector3; }
private Vector3 getVector3(Vector3f angularVelocity) { Vector3 rosVector3 = newMessageFromType(Vector3._TYPE); RosTools.packVector3fToGeometry_msgsVector3(angularVelocity, rosVector3); return rosVector3; }
private Header createHeaderMsg(long timestamp) { Header header = newMessageFromType(Header._TYPE); header.setStamp(Time.fromNano(timestamp)); header.setSeq(counter); counter++; return header; }
private Header createHeaderMsg(long timestamp) { Header header = newMessageFromType(Header._TYPE); header.setStamp(Time.fromNano(timestamp)); header.setSeq(counter); counter++; return header; }