public void OnUpdateRobotLocation(Location location)
{
RobotLocation = location;
OnUpdateHistoricalLocation(location);
OnUpdateRobotLocationEvent?.Invoke(this, location);
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void OnLidarProcessedLineReceived(object sender, List <SegmentExtended> list_of_segments)
{
List <SegmentExtended> corrected_list_segment = new List <SegmentExtended>();
foreach (SegmentExtended segment in list_of_segments)
{
PolarPointRssi point_a = Toolbox.ConvertPointDToPolar(new PointD(segment.Segment.X1, segment.Segment.Y1));
PolarPointRssi point_b = Toolbox.ConvertPointDToPolar(new PointD(segment.Segment.X2, segment.Segment.Y2));
PointD correct_point_a = new PointD(
RobotLocation.X + (point_a.Distance * Math.Cos(RobotLocation.Theta + point_a.Angle)),
RobotLocation.Y + (point_a.Distance * Math.Sin(RobotLocation.Theta + point_a.Angle))
);
PointD correct_point_b = new PointD(
RobotLocation.X + (point_b.Distance * Math.Cos(RobotLocation.Theta + point_b.Angle)),
RobotLocation.Y + (point_b.Distance * Math.Sin(RobotLocation.Theta + point_b.Angle))
);
corrected_list_segment.Add(new SegmentExtended(correct_point_a, correct_point_b, segment.Color, segment.Width));
}
LidarSegment = corrected_list_segment;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void Init()
{
RobotLocation = new Location(0, 0, 0, 0, 0, 0);
RobotGhostLocation = new Location(0, 0, 0, 0, 0, 0);
WaypointLocations = new List <PointD> {
};
RobotHistorical = new List <Location> {
RobotLocation
};
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void ResetRobot(Location location)
{
RobotLocation = location;
RobotGhostLocation = location;
WaypointLocations = new List <PointD> {
};
RobotHistorical = new List <Location> {
RobotLocation
};
OnResetRobotEvent?.Invoke(this, location);
OnLocalWorldMapEvent?.Invoke(this, this);
}
// public virtual List<PointD> lidarMap { get; set; }
// public virtual List<PointD> lidarMapProcessed { get; set; }
// public virtual Heatmap heatMapStrategy { get; set; }
// public virtual Heatmap heatMapWaypoint { get; set; }
#endregion
#region Constructors
public LocalWorldMap(int robotId, int teamId)
{
RobotId = robotId;
TeamId = teamId;
RobotLocation = new Location(0, 0, 0, 0, 0, 0);
RobotGhostLocation = new Location(0, 0, 0, 0, 0, 0);
WaypointLocations = new List <PointD> {
};
RobotHistorical = new List <Location> {
RobotLocation
};
OnLocalWorldMapEvent?.Invoke(this, this);
LidarCup = new List <Cup>();
}
public void OnLidarProcessedPointReceived(object sender, List <PolarPointRssiExtended> lidarPoints)
{
LidarMapProcessed = lidarPoints.Select(
x => new PointDExtended(
new PointD(
RobotLocation.X + (x.Pt.Distance * Math.Cos(RobotLocation.Theta + x.Pt.Angle)),
RobotLocation.Y + (x.Pt.Distance * Math.Sin(RobotLocation.Theta + x.Pt.Angle))
),
x.Color,
x.Width
)
).ToList();
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void OnUpdateHistoricalLocation(Location location)
{
if (RobotHistorical.Count != 0)
{
Location lastHistorical = RobotHistorical[RobotHistorical.Count - 1];
PointD p1 = new PointD(location.X, location.Y);
PointD p2 = new PointD(lastHistorical.X, lastHistorical.Y);
double distance = Toolbox.Distance(p1, p2);
if (distance >= MINIMAL_WORLD_HISTORICAL_DIST)
{
RobotHistorical.Add(location);
OnNewHistoricalPositionEvent?.Invoke(this, location);
}
}
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void OnGameStateChange(object sender, GameState gameState_a)
{
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void OnLidarProcesObjectsReceived(object sender, List <LidarObjects> lidarObjects)
{
LidarObjectList = lidarObjects;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void OnLidarProcessedCupReceived(object sender, List <Cup> cups)
{
LidarCup = cups;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void SetGhostRobotLocation(Location location)
{
RobotGhostLocation = location;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void DeleteFirstWaypoint()
{
WaypointLocations.RemoveAt(0);
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void ResetDestination()
{
DestinationLocation = null;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void ResetWaypoints()
{
WaypointLocations = new List <PointD> {
};
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void SetDestinationLocation(Location location)
{
DestinationLocation = location;
OnLocalWorldMapEvent?.Invoke(this, this);
}
public void AddNewWaypoints(PointD location)
{
WaypointLocations.Add(location);
OnLocalWorldMapEvent?.Invoke(this, this);
}