public void initAutonomousNav()
{
if (m_Robot == null)
m_Map = new Map(1024, 1024, 0.1f);
else
m_Map = m_Robot.Map;
m_History = new List<PointF>();
m_Scale = 0.05f;
m_PathPen = new Pen(Color.Black, 2.0f);
m_HistoryBrush = new SolidBrush(Color.Black);
m_NoScanWaypointBrush = new SolidBrush(Color.Black);
m_DirectionalScanWaypointBrush = new SolidBrush(Color.Green);
m_FullScanWaypointBrush = new SolidBrush(Color.Blue);
m_RobotBrush = new SolidBrush(Color.DarkGray);
m_BlackPen = new Pen(Color.Black, 2.0f);
m_Path = new Path();
m_MinorGridPen = new Pen(Color.Silver, 1.0f);
m_MinorGridPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
m_MajorGridPen = new Pen(Color.Black, 1.0f);
m_MajorGridPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash;
m_IndexToMove = -1;
m_MovingPoint = false;
UpdateMap();
if (m_Robot != null)
ScrollToPosition(m_Robot.Telemetry.Position);
}
/* The collection of telemetry data */
protected void TelemetryMain()
{
while (Connected)
{
try
{
byte[] Response = SendMessage(0x0000000c, null);
GCHandle hBuffer = GCHandle.Alloc(Response, GCHandleType.Pinned);
IntPtr Address = hBuffer.AddrOfPinnedObject();
Telemetry T = (Telemetry)Marshal.PtrToStructure(Address, typeof(Telemetry));
hBuffer.Free();
lock (this)
{
m_LastTelemetryUpdate = DateTime.Now;
m_TelemetryHistory.Add(m_Telemetry);
m_Telemetry = T;
}
int Offset = Marshal.SizeOf(T);
if (T.ImageDataLength > 0)
{
Image I = Image.Read(Response, Offset, T.ImageDataLength);
lock (this)
m_Images.Add(I);
Offset += T.ImageDataLength;
}
if (T.ScanDataLength > 0)
{
RangeImage I = RangeImage.Read(Response, Offset, T.ScanDataLength);
lock (this)
{
m_RangeImages.Add(I);
}
Offset += T.ScanDataLength;
}
if (T.PathDataLength > 0)
{
Offset += T.PathDataLength;
}
if (T.MapDataLength > 0)
{
Map Map = Map.Read(Response, Offset, T.MapDataLength);
lock (this)
m_Map = Map;
Offset += T.MapDataLength;
}
Thread.Sleep(1000);
}
catch (SocketException E)
{
ForceDisconnect();
}
}
}
public Navigation(Robot R)
{
InitializeComponent();
m_Robot = R;
if (m_Robot == null)
m_Map = new Map(1024, 1024, 0.1f);
else
m_Map = m_Robot.Map;
m_History = new List<PointF>();
m_Scale = 0.05f;
m_PathPen = new Pen(Color.Black, 2.0f);
m_HistoryBrush = new SolidBrush(Color.Black);
m_NoScanWaypointBrush = new SolidBrush(Color.Black);
m_DirectionalScanWaypointBrush = new SolidBrush(Color.Green);
m_FullScanWaypointBrush = new SolidBrush(Color.Blue);
m_RobotBrush = new SolidBrush(Color.DarkGray);
m_BlackPen = new Pen(Color.Black, 2.0f);
m_Path = new Path();
m_MinorGridPen = new Pen(Color.Silver, 1.0f);
m_MinorGridPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dot;
m_MajorGridPen = new Pen(Color.Black, 1.0f);
m_MajorGridPen.DashStyle = System.Drawing.Drawing2D.DashStyle.Dash;
m_IndexToMove = -1;
m_MovingPoint = false;
UpdateMap();
if (m_Robot != null)
ScrollToPosition(m_Robot.Telemetry.Position);
//---------------------
//For RangeImage Viewer
//---------------------
m_DrawStyle = DrawStyle.TrianglesColorByDistance;
scanParams = new ScanParameters();
// these should probably be read in from a file or something...
scanParams.HRange = 0;
scanParams.HRes = 0;
scanParams.scanBottom = -45;
scanParams.scanLines = 360;
scanParams.scanRes = (float).25;
//InitGraphics();
InitializeComponent();
}
/* Constructs a new Robot interface class. Connects to a given host and port */
public Robot(string Host, int Port)
{
m_Host = Host;
m_Port = Port;
m_Map = new Map(1024, 1024, 0.05f);
Connect();
}
/**
* @brief Reads the given reader.
* @exception "InvalidOperationException" @brief Thrown when the requested operation is invalid.
* @param Reader A BinaryReader
* @returns A Map.
*/
public static Map Read(BinaryReader Reader)
{
Map M = new Map();
UInt32 Version = Reader.ReadUInt32();
if (Version != 0x101)
throw new InvalidOperationException("The Map format is too old");
M.m_Width = Reader.ReadInt32();
M.m_Height = Reader.ReadInt32();
M.m_Resolution = Reader.ReadSingle();
M.m_MinHeight = Reader.ReadSingle();
M.m_MaxHeight = Reader.ReadSingle();
M.m_Cells = new MapCell[M.m_Width * M.m_Height];
byte[] B = Reader.ReadBytes(M.m_Width * M.m_Height * Marshal.SizeOf(typeof(MapCell)));
GCHandle hData = GCHandle.Alloc(M.m_Cells, GCHandleType.Pinned);
IntPtr pData = hData.AddrOfPinnedObject();
Marshal.Copy(B, 0, pData, B.Length);
hData.Free();
return M;
}
