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; }