public void HandleSubmapMessage(SubmapCloudMsg msg) { if (pending_submap != null) { GameObject obj; SubmapVisualizer vis; if (!submaps.ContainsKey(pending_submap.submap_index)) { obj = new GameObject(); obj.transform.parent = transform; vis = obj.AddComponent <SubmapVisualizer>(); submaps.Add(pending_submap.submap_index, vis); } else { vis = submaps[pending_submap.submap_index]; obj = vis.gameObject; } obj.transform.position = this.transform.position + (new Vector3( pending_submap.pose._position.GetX(), pending_submap.pose._position.GetY(), pending_submap.pose._position.GetZ())); obj.transform.rotation = this.transform.rotation * (new Quaternion( pending_submap.pose._orientation.GetX(), pending_submap.pose._orientation.GetY(), pending_submap.pose._orientation.GetZ(), pending_submap.pose._orientation.GetW())); vis.UpdateMap(msg.cloud.GetCloud()); submap_versions[pending_submap.submap_index] = pending_submap.submap_version; pending_submap = null; } }
void Update() { if (pending_submap == null) { int minIndex = -1; int maxDifference = 0; SubmapEntryMsg needed_submap = null; foreach (KeyValuePair <int, SubmapEntryMsg> curr in newest_version) { if (!submaps.ContainsKey(curr.Key)) { minIndex = curr.Key; needed_submap = curr.Value; break; } int currDiff = curr.Value.submap_version - submap_versions[curr.Key]; if (currDiff > maxDifference) { minIndex = curr.Key; maxDifference = currDiff; needed_submap = curr.Value; } } if (minIndex >= 0) { pending_submap = needed_submap; //Debug.Log("Requestiong Submap: " + pending_submap.submap_index); connection.ros.CallService(SubmapServiceResponse.GetServiceName(), string.Format("[0, {0}, {1}, false]", minIndex, threshold_probability)); } } }
public void HandleSubmapEntry(SubmapEntryMsg msg) { if (!newest_version.ContainsKey(msg.submap_index) || msg.submap_version > newest_version[msg.submap_index].submap_version) { newest_version[msg.submap_index] = msg; } }