private void PlotPath(IPath path)
{
var pathTransform = transform.Find("Path");
if (pathTransform != null)
{
DestroyImmediate(pathTransform.gameObject);
}
var pathObj = new GameObject("Path");
var pathToPlot = path.Clone();
pathToPlot = RemoveEdges(pathToPlot);
if (pathToPlot.Size > 0)
{
var map = GameObject.Find("MapBuilder").GetComponent <MapBuilder>();
var from = map.GridToSpace(pathToPlot.Front);
var pointObj = Instantiate(_pathPointPrefab);
pointObj.transform.position = from;
pointObj.transform.SetParent(pathObj.transform);
pathToPlot.PopFront();
while (pathToPlot.Size > 0)
{
var location = pathToPlot.Front;
var pos = map.GridToSpace(location);
pointObj = Instantiate(_pathPointPrefab);
pointObj.transform.position = pos;
pointObj.transform.SetParent(pathObj.transform);
var lineObj = Instantiate(_pathLinePrefab);
var line = lineObj.GetComponent <LineRenderer>();
line.positionCount = 2;
line.SetPosition(0, from);
line.SetPosition(1, pos);
lineObj.transform.SetParent(pathObj.transform);
pathToPlot.PopFront();
from = pos;
}
}
pathObj.transform.SetParent(transform);
}
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List <Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List <Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}
public RobotState(int robotID, RobotMode mode, RobotSubMode subMode, RobotPose pose, RobotPose rpose,
bool poseWatchdogFail, bool plannerFail, bool inObstacle, bool wpInObstacle, bool isLockedOn,
RobotNeutralizeStatus neutralizeStatus, List<Vector2> waypoints, double angleWhenDone,
IPath path, IPath sparsePath, RobotTwoWheelCommand cmd, DateTime cmdTime, int waypointsCompleted)
{
RobotID = robotID;
Mode = mode;
SubMode = subMode;
Pose = pose.DeepCopy();
rPose = rpose.DeepCopy();
PoseWatchdogFail = poseWatchdogFail;
PlannerFail = plannerFail;
InObstacle = inObstacle;
WpInObstacle = wpInObstacle;
IsLockedOn = isLockedOn;
NeutralizeStatus = neutralizeStatus;
Waypoints = new List<Vector2>(waypoints);
AngleWhenDone = angleWhenDone;
TrackedPath = path.Clone();
SparsePath = sparsePath.Clone();
Cmd = new RobotTwoWheelCommand(cmd.velocity, cmd.turn);
CmdTime = new DateTime(cmdTime.Ticks);
WaypointsCompleted = waypointsCompleted;
}