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 TimeAndFrameID getLatestTimeAndParent()
{
lock (storage)
{
if (storage.Count == 0)
{
return(new TimeAndFrameID(0, 0));
}
TransformStorage ts = storage.Last().Value;
return(new TimeAndFrameID(ts.stamp, ts.frame_id));
}
}
public uint getParent(ulong time, ref string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes = findClosest(ref temp1, ref temp2, time, ref error_str);
if (num_nodes == 0)
{
return(0);
}
return(temp1.frame_id);
}
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);
}
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;
}
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 getData(TimeData time_, ref TransformStorage data_out, ref string error_str)
{
return(getData(toLong(time_), ref data_out, ref error_str));
}
