public bool getData(ulong time_, ref TransformStorage data_out, ref string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes;
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()
{
if (storage.Count == 0)
{
return(new TimeAndFrameID(0, 0));
}
TransformStorage ts = storage.Back;
return(new TimeAndFrameID(ts.stamp, ts.frame_id));
}
public bool insertData(TransformStorage new_data)
{
if (storage.Count > 0 && storage.Front.stamp > new_data.stamp + max_storage_time)
{
return(false);
}
storage.insert(new_data, (a, b) => a.stamp > new_data.stamp);
pruneList();
return(true);
}
public uint getParent(ulong time, ref string error_str)
{
TransformStorage temp1 = null, temp2 = null;
int num_nodes;
num_nodes = findClosest(ref temp1, ref temp2, time, ref error_str);
if (num_nodes == 0)
{
return(0);
}
return(temp1.frame_id);
}
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;
}
public bool getData(TimeData time_, ref TransformStorage data_out, ref string error_str)
{
return(getData(toLong(time_), ref data_out, ref error_str));
}
private byte findClosest(ref TransformStorage one, ref TransformStorage two, ulong target_time, ref string error_str)
{
if (storage.Count == 0)
{
createEmptyException(ref error_str);
return(0);
}
if (target_time == 0)
{
one = storage.Back;
return(1);
}
if (storage.Count == 1)
{
TransformStorage ts = storage.Front;
if (ts.stamp == target_time)
{
one = ts;
return(1);
}
createExtrapolationException1(target_time, ts.stamp, ref error_str);
return(0);
}
ulong latest_time = storage.Back.stamp;
ulong earliest_time = storage.Front.stamp;
if (target_time == latest_time)
{
one = storage.Back;
return(1);
}
if (target_time == earliest_time)
{
one = storage.Front;
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;
for (i = 0; i < storage.Count; i++)
{
if (storage[i].stamp <= target_time)
{
break;
}
}
one = storage[i + 1];
two = storage[i];
return(2);
}
