//public Map_saver(NodeHandle nh)
//{
// this.rosNode = nh;
//}
//bool isSavedMap = false;
//NodeHandle rosNode;
/// <summary>
/// 保存地图数据
/// </summary>
/// <param name="mapName"></param>
/// <param name="path"></param>
public static void SaveMap(string mapName, string path, nm.OccupancyGrid m)
{
//isSavedMap = false;
//int count = 0;
//Subscriber<nm.OccupancyGrid> mapSub = rosNode.subscribe<nm.OccupancyGrid>("/map", 1, (m) =>
//{
string mapdatafile = mapName + ".pgm";
using (BinaryWriter file = new BinaryWriter(File.Open(System.IO.Path.Combine(path, mapdatafile), FileMode.Create), Encoding.ASCII))
{
file.Write(string.Format("P5\n# CREATOR:Map_saver.cs {0:f3} m/pix\n{1} {2}\n255\n", m.info.resolution, m.info.width, m.info.height).ToCharArray());
for (uint y = 0; y < m.info.height; y++)
{
for (uint x = 0; x < m.info.width; x++)
{
uint i = x + (m.info.height - y - 1) * m.info.width;
if (m.data[i] == 0)
{
file.Write((byte)254);
}
else if (m.data[i] == +100)
{
file.Write((byte)000);
}
else
{
file.Write((byte)205);
}
}
}
file.Flush();
file.Close();
string mapmetadatafile = mapName + ".yaml";
using (BinaryWriter metafile = new BinaryWriter(File.Open(Path.Combine(path, mapmetadatafile), FileMode.Create), Encoding.ASCII))
{
emQuaternion orientation = new emQuaternion(m.info.origin.orientation);
emVector3 rpy = orientation.getRPY();
metafile.Write(string.Format("image: {0}\nresolution: {1:f}\norigin: [{2},{3},{4}]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n\n",
mapdatafile, m.info.resolution, m.info.origin.position.x, m.info.origin.position.y, rpy.z).ToCharArray());
metafile.Flush();
metafile.Close();
}
//isSavedMap = true;
}
//});
//while(!isSavedMap)
//{
// if(count++>20)
// {
// throw new Exception("保存地图超时！");
// break;
// }
// System.Threading.Thread.Sleep(500);
//}
//mapSub.shutdown();
}
private void imu_callback(sm.Imu i)
{
emQuaternion q = new emQuaternion(i.orientation);
emVector3 rot = q.getRPY();
//Console.WriteLine("" + euler.roll + " " + euler.pitch + " " + euler.yaw);
Dispatcher.BeginInvoke(new Action(() =>
{
if (abraCadabra.Visibility != Visibility.Visible)
{
abraCadabra.Visibility = Visibility.Visible;
}
rotate(rot.y);
translate(rot.x);
}));
}
// Update is called once per frame
void Update()
{
#region SHOULD I be Recycled?
if (decay > 0.0001 && (Time.fixedTime - lastUpdate) > decay)
{
if (lastRecycle < 0.0001f)
{
lastRecycle = Time.fixedTime;
}
if ((Time.fixedTime - lastRecycle) > (decay + 1f))
{
expire();
return;
}
lastRecycle = Time.fixedTime;
recycle();
return;
}
#endregion
if (changed)
{
//show if hidden (this scan was recycled)
//hideFlags &= ~HideFlags.HideAndDontSave;
#region RESIZE IF NEEDED, ADD+REMOVE SPHERES AS NEEDED
//resize sphere array if different from distbuffer
//remath all circles based on distBuffer
if (pointBuffer != null && pointBuffer.Length != distBuffer.Length)
{
int oldsize = pointBuffer.Length;
int newsize = distBuffer.Length;
if (oldsize < newsize)
{
Array.Resize(ref pointBuffer, newsize);
for (int i = oldsize; i < newsize; i++)
{
GameObject newsphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
newsphere.transform.SetParent(transform);
pointBuffer[i] = newsphere;
}
}
else
{
for (int i = oldsize; i >= newsize; i--)
{
pointBuffer[i].transform.SetParent(null);
pointBuffer[i] = null;
}
Array.Resize(ref pointBuffer, newsize);
}
}
else if (pointBuffer == null)
{
pointBuffer = new GameObject[distBuffer.Length];
for (int i = 0; i < distBuffer.Length; i++)
{
GameObject newsphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
newsphere.transform.SetParent(transform);
pointBuffer[i] = newsphere;
}
}
#endregion
#region FOR ALL SPHERES ALL THE TIME
for (int i = 0; i < pointBuffer.Length; i++)
{
pointBuffer[i].SetActive(false);
if (distBuffer[i] > (maxRange - 0.0001f) || distBuffer[i] < (minRange + 0.0001f))
{
continue;
}
pointBuffer[i].transform.localScale = new Vector3(pointSize, pointSize, pointSize);
Vector3 parentPos = TF.position;
emQuaternion rot = new emQuaternion(0, 0, 0, 1);
emVector3 pos = new emVector3((float)(distBuffer[i] * Math.Cos(angMin + angInc * i)), (float)(distBuffer[i] * Math.Sin(angMin + angInc * i)), 0f);
pointBuffer[i].transform.localPosition = pos.UnityPosition;
pointBuffer[i].transform.localRotation = rot.UnityRotation;
pointBuffer[i].GetComponent <MeshRenderer>().material.color = Color;
pointBuffer[i].SetActive(true);
}
//save position, reset pos of scan views to be their old pose until updates finish
//gameObject.SetActive(false);
transform.position = TF.position;
transform.rotation = TF.rotation;
//gameObject.SetActive(true);
#endregion
changed = false;
}
}
/// <summary>
/// 订阅更新话题
/// </summary>
/// <param name="topic"></param>
public void SubscribeToPostion(string topic)
{
lock (this)
{
if (foothandle == null)
{
foothandle = new NodeHandle();
}
if (footSub != null && footSub.topic != topic)
{
footSub.shutdown();
footSub = null;
}
if (footSub != null)
{
return;
}
if (tfer == null)
{
tfer = new Transformer(false);
}
Console.WriteLine("Subscribing to robot_baselink at:= " + topic);
footSub = foothandle.subscribe <gm.PoseStamped>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
//if (i.polygon.points.Length == 0) return;
//if (!tfer.waitForTransform("/map", "/base", i.header.stamp, new m.Duration(new Messages.TimeData(1, 0)), null))
// return ;
//tfer.transformVector("/map",)
//emTransform ret = new emTransform();
//if (tfer.lookupTransform("/map", "/base", i.header.stamp, out ret))
//{
pointArray.Clear();
PointF[] vecs = new PointF[3];
emVector3 orien = (new emQuaternion(i.pose.orientation)).getRPY();
double d1 = 0.3, d2 = 0.1;
vecs[0] = new PointF((float)(i.pose.position.x + d1 * Math.Cos(orien.z)), (float)(i.pose.position.y + d1 * Math.Sin(orien.z)));
vecs[1] = new PointF((float)(i.pose.position.x + d2 * Math.Cos(orien.z + Math.PI / 2)), (float)(i.pose.position.y + d2 * Math.Sin(orien.z + Math.PI / 2)));
vecs[2] = new PointF((float)(i.pose.position.x + d2 * Math.Cos(orien.z - Math.PI / 2)), (float)(i.pose.position.y + d2 * Math.Sin(orien.z - Math.PI / 2)));
int length = vecs.Length;
for (int index = 0; index < length; index++)
{
//Stamped<emVector3> stampVector_in = new Stamped<emVector3>(i.header.stamp, i.header.frame_id, new emVector3(p.x,p.y,p.z));
//Stamped<emVector3> stamVector_out = new Stamped<emVector3>();
//tfer.transformVector("map", stampVector_in, ref stamVector_out);
Point worldPoint = owner.Map2World(vecs[index]);
pointArray.Add(worldPoint);
}
pointArray.Add(pointArray[0]);
if (length > 0)
{
startPoint = (Point)pointArray[0];
endPoint = (Point)pointArray[length - 1];
}
Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
//owner.Invalidate();
//}
})));
}
}
