public void Add(DateTime timeStamp, Pose pose)
{
m_UtcTimeStamps.Add(timeStamp.ToUniversalTime());
m_Poses.Add(pose);
if (pose.X < m_MinX)
{
m_MinX = pose.X;
}
if (pose.X > m_MaxX)
{
m_MaxX = pose.X;
}
if (pose.Y < m_MinY)
{
m_MinY = pose.Y;
}
if (pose.Y > m_MaxY)
{
m_MaxY = pose.Y;
}
if (m_Poses.Count > 1)
{
double dx = m_Poses[m_Poses.Count - 2].X - pose.X;
double dy = m_Poses[m_Poses.Count - 2].Y - pose.Y;
m_PathLength += Math.Sqrt(dx * dx + dy * dy);
}
}
private void RaiseNewPoseEvent()
{
var handler = NewPose;
if (handler != null)
{
Pose newPose = new Pose(m_RobotPose.Location, m_RobotPose.Heading);
var e = new PoseEventArgs(newPose);
handler(this, e);
}
}
public PoseEventArgs(Pose pose)
{
this.Pose = pose;
}
public Pose(Pose pose)
{
this.Location = pose.Location;
this.Heading = pose.Heading; ;
}
public void Add(Pose currentPose)
{
Add(DateTime.UtcNow, currentPose);
}