public void Publish()
{
Vector3 mpos = mobileBase.transform.position;
Vector3 tpos = targetCube.transform.position;
float angle = Mathf.Atan2(tpos.z - mpos.z, tpos.x - mpos.x);
BasePose basePose = new BasePose(tpos.x, tpos.y, tpos.z, angle);
ros.Send(topicName, basePose);
}
public override void Update()
{
if (BasePose == null || ReferencePose == null || AddPose == null)
{
return;
}
var addPose = AddPose.Clone();
var refPose = ReferencePose.Clone();
var basePose = BasePose.Clone();
var tempPose = BasePose.Clone();
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(addPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(refPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.MinusPose(tempPose, refPose, addPose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToMeshSpace(basePose);
Bricks.Animation.Runtime.CGfxAnimationRuntime.AddPose(OutPose, basePose, tempPose, 1);
Bricks.Animation.Runtime.CGfxAnimationRuntime.ConvertRotationToLocalSpace(OutPose);
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < publishMessageFrequency)
{
return;
}
Transform tf = mobileBase.transform;
Vector3 pos = tf.position;
Vector3 pos_d = pos + tf.forward.normalized;
float angle = Mathf.Atan2(pos_d.z - pos.z, pos_d.x - pos.x);
BasePose basePose = new BasePose(pos.x, pos.y, pos.z, angle);
ros.Send(topicName, basePose);
timeElapsed = 0;
}
