public void AddTransform(string name, Transform transform, bool force = false)
{
if (!tfStorage.ContainsKey(name))
{
tfStorage[name] = new TransformStorage {
transform = transform
};
return;
}
if (force)
{
var transStorage = tfStorage[name];
transStorage.transform = transform;
if (transStorage.delegates != null)
{
transStorage.delegates(transform);
}
Debug.LogWarning(
"A key with name [" + name + "] was already added," +
" but it was force-overwritten."
);
return;
}
Debug.LogWarning(
"A key named [" + name + "] already existed and" +
" was not force-overwritten."
);
}
private void Awake()
{
_previousTransform = new TransformStorage
{
Position = transform.position,
Rotation = transform.rotation,
LocalScale = transform.localScale
};
CurrentStatus = Status.Idle;
MoveDirection = MovingTo.None;
// refresh status at frequency
InvokeRepeating("RefreshStatus", RefreshStatusFrequency,
RefreshStatusFrequency);
}
/// <summary>
/// Gets a named transform and calls the delegate. You can retrieve
/// it via the callback or just by the return value.
/// The delegate gets called when the storage gets updated with that name.
/// </summary>
public Transform GetTransform(string name, TransformUpdate tuDelegate)
{
var transStorage = tfStorage[name];
if (transStorage == null)
{
tfStorage[name] = new TransformStorage {
delegates = tuDelegate
};
return(null);
}
transStorage.delegates += tuDelegate;
tuDelegate(transStorage.transform);
return(transStorage.transform);
}
private byte findClosest(ref TransformStorage one, ref TransformStorage two, ulong target_time, ref string error_str)
{
lock (storage)
{
if (storage.Count == 0)
{
createEmptyException(ref error_str);
return 0;
}
if (target_time == 0)
{
one = storage.Last().Value;
return 1;
}
if (storage.Count == 1)
{
TransformStorage ts = storage.First().Value;
if (ts.stamp == target_time)
{
one = ts;
return 1;
}
createExtrapolationException1(target_time, ts.stamp, ref error_str);
return 0;
}
ulong latest_time = storage.Last().Key;
ulong earliest_time = storage.First().Key;
if (target_time == latest_time)
{
one = storage.Last().Value;
return 1;
}
if (target_time == earliest_time)
{
one = storage.First().Value;
return 1;
}
if (target_time > latest_time)
{
createExtrapolationException2(target_time, latest_time, ref error_str);
return 0;
}
if (target_time < earliest_time)
{
createExtrapolationException3(target_time, earliest_time, ref error_str);
return 0;
}
ulong i = 0;
ulong j = storage.Last((kvp) =>
{
//look for the first keyvaluepair in the sorted list with a key greater than our target.
//i is the last keyvaluepair's key, aka, the highest stamp
if (kvp.Key <= target_time)
{
i = kvp.Key;
return false;
}
return true;
}).Key;
one = storage[i];
two = storage[j];
}
return 2;
}
public bool insertData(TransformStorage new_data)
{
lock (storage)
{
if (storage.Count > 0 && storage.First().Key > new_data.stamp + max_storage_time)
if (SimTime.instance.IsTimeSimulated)
{
storage.Clear();
}
else
return false;
storage[new_data.stamp] = new_data;
pruneList();
}
return true;
}
public bool getData(ulong time_, ref TransformStorage data_out, ref string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes = findClosest(ref temp1, ref temp2, time_, ref error_str);
switch (num_nodes)
{
case 0:
return false;
case 1:
data_out = temp1;
break;
case 2:
if (temp1.frame_id == temp2.frame_id)
{
interpolate(temp1, temp2, time_, ref data_out);
}
else
{
data_out = temp1;
}
break;
default:
ROS.FREAKOUT();
break;
}
return true;
}
public bool getData(TimeData time_, ref TransformStorage data_out, ref string error_str)
{
return getData(toLong(time_), ref data_out, ref error_str);
}
private void interpolate(TransformStorage one, TransformStorage two, ulong time, ref TransformStorage output)
{
if (one.stamp == two.stamp)
{
output = two;
return;
}
if (output == null)
output = new TransformStorage();
double ratio = (time - one.stamp) / (two.stamp - one.stamp);
output.translation.setInterpolate3(one.translation, two.translation, ratio);
output.rotation = slerp(one.rotation, two.rotation, ratio);
output.stamp = one.stamp;
output.frame_id = one.frame_id;
output.child_frame_id = one.child_frame_id;
}
