public void OnAbsolutePositionCalculatedEvent(object sender, PositionArgs e)
{
currentGpsXRefTerrain = e.X;
currentGpsYRefTerrain = e.Y;
currentGpsTheta = Toolbox.ModuloByAngle(kalmanLocationRefTerrain.Theta, e.Theta);
}
private IEnumerator ReachTarget(EventArgs args)
{
PositionArgs pArgs = args as PositionArgs;
if (NavMesh.CalculatePath(m_containerAttachedTo.View.transform.position, pArgs.position, NavMesh.AllAreas, path))
{
for (int i = 0; i < path.corners.Length - 1; i++)
{
Debug.DrawLine(path.corners[i], path.corners[i + 1], Color.blue, 1f);
}
}
else
{
Debug.LogWarning("Path not found");
}
while (!IsCloseTo(pArgs.position))
{
yield return(null);
}
DeviceEvent reached = GetEvent("TargetReached");
if (reached != null)
{
m_containerAttachedTo.IntegratedDevice.ScheduleEvent(reached, null);
}
}
public void ReserveOkPartMove(MapSection mapSection)
{
try
{
int sectionIndex = MovingGuide.GuideSectionIds.FindIndex(x => x.Trim() == mapSection.Id.Trim());
if (sectionIndex < 0)
{
OnLogErrorEvent?.Invoke(this, new MessageHandlerArgs()
{
ClassMethodName = GetType().Name + ":" + System.Reflection.MethodBase.GetCurrentMethod().Name,
Message = $"ReserveOkPartMove fail.[{mapSection.Id.Trim()}].[{MovingGuide.GuideSectionIds.GetJsonInfo()}]"
});
return;
}
MapAddress address = MapInfo.addressMap[MovingGuide.GuideAddressIds[sectionIndex + 1]];
PositionArgs positionArgs = new PositionArgs()
{
EnumLocalArrival = sectionIndex == MovingGuide.GuideSectionIds.Count - 1 ? EnumLocalArrival.EndArrival : EnumLocalArrival.Arrival,
MapPosition = address.Position
};
FakeMoveArrivalQueue.Enqueue(positionArgs);
}
catch (Exception ex)
{
OnLogErrorEvent?.Invoke(this, new MessageHandlerArgs()
{
ClassMethodName = GetType().Name + ":" + System.Reflection.MethodBase.GetCurrentMethod().Name,
Message = ex.Message
});
}
}
public void OnWaypointReceived(object sender, PositionArgs destination)
{
/// Mise à jour du waypoint courant
wayPointLocation.X = destination.X;
wayPointLocation.Y = destination.Y;
/// Initialisation de la machine à état de déplacement du Ghost
}
private void model_PointClicked(object sender, PositionArgs e)
{
var best = FindFeature(e.X);
if (best != null)
{
LoadSpectrum(best);
}
}
private void WorldMapDisplay_OnCtrlClickOnHeatMapEvent(object sender, PositionArgs e)
{
//RefBoxMessage msg = new RefBoxMessage();
//msg.command = RefBoxCommand.GOTO;
//msg.targetTeam = TeamIpAddress;
//msg.robotID = (int)TeamId.Team1 + (int)RobotId.Robot1;
//msg.posX = e.X;
//msg.posY = e.Y;
//msg.posTheta = 0;
//OnRefereeBoxReceivedCommand(msg);
}
public IEnumerator SteerTowards(EventArgs args)
{
PositionArgs pArgs = args as PositionArgs;
DeviceEvent onSteerStart = GetEvent("OnSteerStart");
if (onSteerStart != null)
{
m_containerAttachedTo.IntegratedDevice.ScheduleEvent(onSteerStart, null);
}
// Vector2 worldPos = (Vector2) pArgs.position;
Vector3 worldPos = pArgs.position;
// m_rigidbody.angularVelocity = 0f;
// m_rigidbody.rotation = Mathf.Repeat( m_rigidbody.rotation, 360f );
m_rigidbody.angularVelocity = Vector3.zero;
float prevAngle = 1f;
float currentAngle = 0f;
while (Mathf.Abs(currentAngle) < Mathf.Abs(prevAngle))
{
Quaternion targetRotation = Quaternion.LookRotation((worldPos - m_rigidbody.position).normalized);
// prevAngle = Mathf.DeltaAngle( m_rigidbody.rotation , CurrentAngle(worldPos)) ;
prevAngle = Quaternion.Angle(m_rigidbody.rotation, targetRotation);
//Vector3.Angle( m_rigidbody.rotation * Vector3.forward, ( worldPos - m_rigidbody.position).normalized);
// m_rigidbody.rotation =
// Mathf.MoveTowardsAngle( m_rigidbody.rotation,
// CurrentAngle(worldPos),
// torqueSpeed * Time.deltaTime );
m_rigidbody.rotation = Quaternion.RotateTowards(m_rigidbody.rotation, targetRotation, torqueSpeed * Time.deltaTime);
// currentAngle = Mathf.DeltaAngle( m_rigidbody.rotation , CurrentAngle(worldPos)) ;
currentAngle = Quaternion.Angle(m_rigidbody.rotation, targetRotation);
//Vector3.Angle( m_rigidbody.rotation * Vector3.forward, ( worldPos - m_rigidbody.position).normalized);
yield return(null);
}
DeviceEvent onSteerComplete = GetEvent("OnSteerComplete");
if (onSteerComplete != null)
{
m_containerAttachedTo.IntegratedDevice.ScheduleEvent(onSteerComplete, null);
}
}
/// <summary>
/// Gps is ready to use
/// </summary>
/// <param name="sender">Sender object</param>
/// <param name="e">New position</param>
private void Model_GPS_Ready(object sender, PositionArgs e)
{
if (!Model.RunningEnd)
{
var position = new LatLng(e.LocationData.Latitude, e.LocationData.Longitude);
_lastUserPosition = position;
DrawUser(_lastUserPosition);
MoveCamera(position, 16);
}
if (!Model.IsRunning)
{
_mainButton.Text = "Start";
_mainButtonState = MainButtonStates.Start;
}
}
/// <summary>
/// When the user stops.
/// </summary>
/// <param name="sender">Sender object</param>
/// <param name="e">User's position</param>
private void Model_UserStopped(object sender, PositionArgs e)
{
var circleOptions = new CircleOptions();
circleOptions.InvokeCenter(new LatLng(e.LocationData.Latitude, e.LocationData.Longitude));
circleOptions.InvokeRadius(14);
circleOptions.InvokeFillColor(Color.Rgb(213, 52, 58));
circleOptions.InvokeStrokeColor(Color.Black);
circleOptions.InvokeStrokeWidth(1);
circleOptions.InvokeZIndex(10);
RunOnUiThread(() =>
{
_map.AddCircle(circleOptions);
_speedTextView.Text = "0";
});
}
/// <summary>
/// Action position changed
/// </summary>
/// <param name="sender">origin</param>
/// <param name="e">args</param>
private void action(object sender, PositionArgs e)
{
this.oldValue = e.OldPosition;
this.newValue = e.NewPosition;
}
private void OnAbsolutePositionCalculatedEvent(object sender, PositionArgs e)
{
robotPerception.robotAbsoluteLocation = new Location(e.X, e.Y, e.Theta, 0, 0, 0);
OnAbsolutePositionEvent?.Invoke(this, e);
}
private void WorldMapDisplay_OnCtrlClickOnHeatMapEvent(object sender, PositionArgs e)
{
}
private void Model_UserStopped(object sender, PositionArgs e)
{
userstopped = true;
}
private void Model_GPS_Ready(object sender, PositionArgs e)
{
gpsready = true;
}
public void MoveInQueue(object sender, PositionArgs args)
{
character.MoveToPosition(args.position);
}
public virtual void OnCtrlClickOnLocalWorldMap(PositionArgs e)
{
OnCtrlClickOnLocalWorldMapEvent?.Invoke(this, e);
}
public void OnCamLidarSimulatedRobotPositionReceived(object sender, PositionArgs e)
{
currentGpsXRefTerrain = e.X;
currentGpsYRefTerrain = e.Y;
currentGpsTheta = e.Theta;
}
/// <summary>
/// Add new point to the polyline
/// </summary>
/// <param name="sender">Sender object</param>
/// <param name="e">User's new position</param>
private void Model_NewPosition(object sender, PositionArgs e)
{
var position = new LatLng(e.LocationData.Latitude, e.LocationData.Longitude);
// Refresh texts
_speedTextView.Text = Math.Round(e.LocationData.Speed, 2).ToString(CultureInfo.InvariantCulture);
_sumDistance = _sumDistance + e.LocationData.Distance;
_distanceTextView.Text = Math.Round(_sumDistance, 2).ToString(CultureInfo.InvariantCulture);
// Add new line
if (e.LocationData.RunningSpeedType != _lastRunningSpeedType)
{
LatLng lastPosition = null;
if (_previousStateOfCurrentPolyline != null)
{
lastPosition = _polylineOptions.Points.Last();
}
_previousStateOfCurrentPolyline = null;
_lastRunningSpeedType = e.LocationData.RunningSpeedType;
_polylineOptions = new PolylineOptions();
switch (e.LocationData.RunningSpeedType)
{
case RunningSpeed.Slow:
_polylineOptions.InvokeColor(Color.Rgb(238, 163, 54));
break;
case RunningSpeed.Normal:
_polylineOptions.InvokeColor(Color.Rgb(51, 127, 192));
break;
case RunningSpeed.Fast:
_polylineOptions.InvokeColor(Color.Rgb(33, 175, 95));
break;
case RunningSpeed.StartPoint:
break;
default:
throw new ArgumentOutOfRangeException();
}
_polylineOptions.InvokeZIndex(5);
if (lastPosition != null)
{
_polylineOptions.Add(lastPosition);
}
}
_polylineOptions.Add(position);
// Drawing the line
var timer = new Timer();
timer.Interval = Model.GpsMinTime / 2;
timer.Elapsed += (o, args) =>
{
RunOnUiThread(() =>
{
var currentPolyline = _map.AddPolyline(_polylineOptions);
_previousStateOfCurrentPolyline?.Remove();
_previousStateOfCurrentPolyline = currentPolyline;
});
timer.Stop();
};
timer.Start();
}
/// <summary>
/// Model gets the current position
/// </summary>
private void Model_UserPosition(object sender, PositionArgs e)
{
var previousUserPosition = _lastUserPosition;
_currentPositionTimer.Start(previousUserPosition, new LatLng(e.LocationData.Latitude, e.LocationData.Longitude));
}
/// <summary>
/// Draw the animated user position.
/// </summary>
/// <param name="sender">Sender object</param>
/// <param name="positionArgs">User's position at the current frame</param>
private void CurrentPositionTimerOnElapsed(object sender, PositionArgs positionArgs)
{
_lastUserPosition = new LatLng(positionArgs.LocationData.Latitude, positionArgs.LocationData.Longitude);
DrawUser(_lastUserPosition);
MoveCamera(_lastUserPosition, null);
}
public void OnRobotPositionReceived(object sender, PositionArgs location)
{
RobotLocation = new Location(location.X, location.Y, location.Theta, 0, 0, 0);
}
