//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();
}
/// <summary>
/// Map provider topic
/// </summary>
//public string Topic
//{
// get { return __topic as string; }
// set
// {
// __topic = value;
// SubscribeToMap(__topic);
// }
//}
/// <summary>
/// 采用调用服务方式获取地图数据
/// </summary>
/// <param name="serviceName"></param>
public void GetStaticMap(string serviceName)
{
lock (this)
{
if (imagehandle == null)
{
imagehandle = new NodeHandle();
}
//if(mapServiceClient!=null)
//{
// mapServiceClient.shutdown();
// mapServiceClient = null;
//}
//if (mapServiceClient != null)
// return;
//mapServiceClient = imagehandle.serviceClient<nm.GetMap>(serviceName);
//nm.GetMap srv = new nm.GetMap();
nm.GetMap.Request req = new Messages.nav_msgs.GetMap.Request();
nm.GetMap.Response resp = new Messages.nav_msgs.GetMap.Response();
if (imagehandle.serviceClient <nm.GetMap.Request, nm.GetMap.Response>(serviceName).call(req, ref resp))
{
nm.OccupancyGrid i = resp.map;
mapResolution = i.info.resolution;
owner.MapResolution = mapResolution;
mapHeight = i.info.height;
mapWidth = i.info.width;
origin = new PointF((float)i.info.origin.position.x, (float)i.info.origin.position.y);
Point left_bottomPoint = owner.Map2World(origin);
left_bottomPoint.Offset(-(int)MapHeight, -(int)mapWidth);
SetRectangle(left_bottomPoint.X, left_bottomPoint.Y, (int)mapHeight, (int)MapWidth);
Size size = new Size((int)i.info.width, (int)i.info.height);
byte[] data = createRGBA(i.data);
UpdateImage(data, size, false);
_originalImage = _image;
_image.RotateFlip(RotateFlipType.Rotate270FlipNone);
data = null;
owner.PanX = owner.Width / 2 - (rectangle.Width / 2 + rectangle.Left);
owner.PanY = owner.Height / 2 - (rectangle.Height / 2 + rectangle.Top);
//owner.Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
}
}
}
/// <summary>
/// 订阅更新话题
/// </summary>
/// <param name="topic"></param>
public void SubscribeToMap(string topic)
{
lock (this)
{
if (imagehandle == null)
{
imagehandle = new NodeHandle();
}
if (mapSub != null && mapSub.topic != topic)
{
mapSub.shutdown();
mapSub = null;
}
if (mapSub != null)
{
return;
}
Console.WriteLine("Subscribing to map at:= " + topic);
lastUpdateTime = DateTime.Now;
mapSub = imagehandle.subscribe <nm.OccupancyGrid>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
//if (lastUpdateTime.AddMilliseconds(1000) > DateTime.Now) return;
lastUpdateTime = DateTime.Now;
mapResolution = i.info.resolution;
owner.MapResolution = mapResolution;
mapHeight = i.info.height;
mapWidth = i.info.width;
origin = new PointF((float)i.info.origin.position.x, (float)i.info.origin.position.y);
Point left_bottomPoint = owner.Map2World(origin);
left_bottomPoint.Offset(-(int)MapHeight, -(int)mapWidth);
SetRectangle(left_bottomPoint.X, left_bottomPoint.Y, (int)mapHeight, (int)MapWidth);
//if (Width != 0)
// actualResolution = (mapWidth / (float)Width) * mapResolution;
//else
// actualResolution = (mapWidth / (float)ActualWidth) * mapResolution;
//if (float.IsNaN(actualResolution) || float.IsInfinity(actualResolution))
// actualResolution = 0;
//else
//{
// MatchAspectRatio();
//}
Size size = new Size((int)i.info.width, (int)i.info.height);
byte[] data = createRGBA(i.data);
UpdateImage(data, size, false);
_originalImage = _image;
_image.RotateFlip(RotateFlipType.Rotate270FlipNone);
data = null;
if (occupancyGrid == null)
{
owner.PanX = owner.Width / 2 - (rectangle.Width / 2 + rectangle.Left);
owner.PanY = owner.Height / 2 - (rectangle.Height / 2 + rectangle.Top);
}
this.occupancyGrid = i;
//owner.Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
})));
}
}
