public void TestAnglePositionNeg()
{
AnglePosition a = new AnglePosition(-50, AngleType.Degre);
Assert.AreEqual(-50, a.InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(310, a.InPositiveDegrees, AnglePosition.PRECISION);
}
public void TestAnglePositionCenter()
{
Assert.AreEqual(45, AnglePosition.Center(0, 90));
Assert.AreEqual(45 + 180, AnglePosition.CenterLongArc(0, 90));
Assert.AreEqual(45, AnglePosition.CenterSmallArc(0, 90));
Assert.AreEqual(45 + 180, AnglePosition.Center(90, 0));
Assert.AreEqual(45 + 180, AnglePosition.CenterLongArc(90, 0));
Assert.AreEqual(45, AnglePosition.CenterSmallArc(90, 0));
Assert.AreEqual(30, AnglePosition.Center(-20, 80));
Assert.AreEqual(30 + 180, AnglePosition.CenterLongArc(-20, 80));
Assert.AreEqual(30, AnglePosition.CenterSmallArc(-20, 80));
Assert.AreEqual(30 + 180, AnglePosition.Center(80, -20));
Assert.AreEqual(30 + 180, AnglePosition.CenterLongArc(80, -20));
Assert.AreEqual(30, AnglePosition.CenterSmallArc(80, -20));
Assert.AreEqual(175, AnglePosition.Center(80, 270));
Assert.AreEqual(175, AnglePosition.CenterLongArc(80, 270));
Assert.AreEqual(175 + 180, AnglePosition.CenterSmallArc(80, 270));
Assert.AreEqual(175 + 180, AnglePosition.Center(270, 80));
Assert.AreEqual(175, AnglePosition.CenterLongArc(270, 80));
Assert.AreEqual(175 + 180, AnglePosition.CenterSmallArc(270, 80));
}
private List <RealPoint> ValuesToPositions(List <double> measures, bool limitOnTable, int minDistance, int maxDistance, Position refPosition)
{
List <RealPoint> positions = new List <RealPoint>();
AngleDelta resolution = _scanRange / _pointsCount;
for (int i = 0; i < measures.Count; i++)
{
AnglePosition angle = resolution * i;
if (measures[i] > minDistance && (measures[i] < maxDistance || maxDistance == -1))
{
AnglePosition anglePoint = new AnglePosition(angle.InPositiveRadians - refPosition.Angle.InPositiveRadians - _scanRange.InRadians / 2 - Math.PI / 2, AngleType.Radian);
RealPoint pos = new RealPoint(refPosition.Coordinates.X - anglePoint.Sin * measures[i], refPosition.Coordinates.Y - anglePoint.Cos * measures[i]);
int marge = 20; // Marge en mm de distance de detection à l'exterieur de la table (pour ne pas jeter les mesures de la bordure qui ne collent pas parfaitement)
if (!limitOnTable || (pos.X > -marge && pos.X < GameBoard.Width + marge && pos.Y > -marge && pos.Y < GameBoard.Height + marge))
{
positions.Add(pos);
}
}
}
return(positions);
}
public Trajectory(Trajectory other)
{
_points = new List <RealPoint>(other.Points);
_lines = new List <Segment>(other.Lines);
_startAngle = other.StartAngle;
_endAngle = other.EndAngle;
}
private static Bitmap RotateImage(Bitmap b, AnglePosition angle)
{
//create a new empty bitmap to hold rotated image
Bitmap returnBitmap = new Bitmap(b.Width, b.Height);
try
{
//make a graphics object from the empty bitmap
Graphics g = Graphics.FromImage(returnBitmap);
//move rotation point to center of image
g.TranslateTransform((float)b.Width / 2, (float)b.Height / 2);
//rotate
g.RotateTransform((float)angle);
//move image back
g.TranslateTransform(-(float)b.Width / 2, -(float)b.Height / 2);
//draw passed in image onto graphics object
g.DrawImage(b, 0, 0, b.Width, b.Height);
}
catch (Exception)
{
returnBitmap.Dispose();
return(null);
}
return(returnBitmap);
}
private static void DessineZoneMorte(Graphics g)
{
int farAway = 10000;
if (AllDevices.LidarAvoid != null && AllDevices.LidarAvoid.DeadAngle > 0)
{
AnglePosition debutAngleMort = AllDevices.LidarAvoid.Position.Angle + 180 + new AngleDelta(AllDevices.LidarAvoid.DeadAngle / 2);
AnglePosition finAngleMort = AllDevices.LidarAvoid.Position.Angle + 180 + new AngleDelta(-AllDevices.LidarAvoid.DeadAngle / 2);
List <Point> points = new List <Point>();
points.Add(Scale.RealToScreenPosition(AllDevices.LidarAvoid.Position.Coordinates));
points.Add(Scale.RealToScreenPosition(new Point((int)(AllDevices.LidarAvoid.Position.Coordinates.X + debutAngleMort.Cos * farAway), (int)(AllDevices.LidarAvoid.Position.Coordinates.Y + debutAngleMort.Sin * farAway))));
points.Add(Scale.RealToScreenPosition(new Point((int)(AllDevices.LidarAvoid.Position.Coordinates.X + finAngleMort.Cos * farAway), (int)(AllDevices.LidarAvoid.Position.Coordinates.Y + finAngleMort.Sin * farAway))));
g.IntersectClip(new Rectangle(Scale.RealToScreenPosition(new Point(0, 0)), new Size(Scale.RealToScreenDistance(GameBoard.Width), Scale.RealToScreenDistance(GameBoard.Height))));
g.DrawLine(Pens.Black, points[0], points[1]);
g.DrawLine(Pens.Black, points[0], points[2]);
Brush brush = new SolidBrush(Color.FromArgb(80, Color.Black));
g.FillPolygon(brush, points.ToArray());
brush.Dispose();
g.ResetClip();
}
}
public void TestAnglePositionDegRad()
{
AnglePosition deg = new AnglePosition(90, AngleType.Degre);
AnglePosition rad = new AnglePosition(Math.PI / 2, AngleType.Radian);
Assert.AreEqual(deg, rad);
Assert.AreEqual(deg, 90, AnglePosition.PRECISION);
}
public void TestAnglePositionModulo()
{
AnglePosition a800 = new AnglePosition(800, AngleType.Degre);
AnglePosition a80 = new AnglePosition(80, AngleType.Degre);
Assert.AreEqual(a800, a80);
Assert.AreEqual(80, a800.InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(80, a800.InPositiveDegrees, AnglePosition.PRECISION);
Assert.AreEqual(80.0 / 180.0 * Math.PI, a800.InRadians, AnglePosition.PRECISION);
}
public void TestAnglePositionSub1()
{
AnglePosition a30 = new AnglePosition(30);
AnglePosition a350 = new AnglePosition(350);
AngleDelta da30 = new AngleDelta(30);
AngleDelta da350 = new AngleDelta(350);
Assert.AreEqual(40, (a30 - da350).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(-40, (a350 - da30).InDegrees, AnglePosition.PRECISION);
}
public void TestAnglePositionNotEqual()
{
AnglePosition a1, a2;
a1 = new AnglePosition(85);
a2 = new AnglePosition(86);
Assert.AreNotEqual(a1, a2);
a1 = new AnglePosition(0);
a2 = new AnglePosition(359);
Assert.AreNotEqual(a1, a2);
}
public void TestAnglePositionAdd()
{
AnglePosition a10 = new AnglePosition(10);
AnglePosition a190 = new AnglePosition(190);
AnglePosition a350 = new AnglePosition(350);
AngleDelta da10 = new AngleDelta(10);
AngleDelta da80 = new AngleDelta(80);
AngleDelta da20 = new AngleDelta(20);
Assert.AreEqual(0, (a350 + da10).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(10, (a350 + da20).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(-90, (a190 + da80).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(20, (a10 + da10).InDegrees, AnglePosition.PRECISION);
}
public void GoToAngle(AnglePosition angle, double marge = 0)
{
AngleDelta diff = angle - Position.Angle;
if (Math.Abs(diff.InDegrees) > marge)
{
if (diff.InDegrees > 0)
{
PivotRight(diff.InDegrees);
}
else
{
PivotLeft(-diff.InDegrees);
}
}
}
//Todo:??
public void ProcessPackage(object caller, ReceiveDatagramEventArgs args)
{
switch (args.Datagram)
{
case GimbalPositionDatagram cradleHeadPositionDatagram:
{
Position = cradleHeadPositionDatagram.Position;
break;
}
case GimbalTargetDatagram cradleHeadTargetDatagram:
{
MoveTarget = cradleHeadTargetDatagram.Target;
break;
}
}
}
public void TestAnglePositionSub2()
{
AnglePosition a0 = new AnglePosition(0);
AnglePosition a30 = new AnglePosition(30);
AnglePosition a60 = new AnglePosition(60);
AnglePosition a89 = new AnglePosition(89);
AnglePosition a90 = new AnglePosition(90);
AnglePosition a91 = new AnglePosition(91);
AnglePosition a270 = new AnglePosition(270);
AnglePosition a350 = new AnglePosition(350);
AnglePosition a360 = new AnglePosition(360);
Assert.AreEqual(-40, (a350 - a30).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(40, (a30 - a350).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(30, (a90 - a60).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(-30, (a60 - a90).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(0, (a0 - a360).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(180, (a90 - a270).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(-179, (a91 - a270).InDegrees, AnglePosition.PRECISION);
Assert.AreEqual(179, (a89 - a270).InDegrees, AnglePosition.PRECISION);
}
private List <RealPoint> Detection(Position refPosition)
{
List <RealPoint> positions = new List <RealPoint>();
AngleDelta resolution = _scanRange / _pointsCount;
int dist = 10000;
List <IShape> obstacles = new List <IShape>();
obstacles.AddRange(GameBoard.ObstaclesLidarGround);
for (int i = 0; i < _pointsCount; i++)
{
AnglePosition angle = resolution * i;
AnglePosition anglePoint = new AnglePosition(angle.InPositiveRadians - refPosition.Angle.InPositiveRadians - _scanRange.InRadians / 2 - Math.PI / 2, AngleType.Radian);
RealPoint pos = new RealPoint(refPosition.Coordinates.X - anglePoint.Sin * dist, refPosition.Coordinates.Y - anglePoint.Cos * dist);
positions.Add(GetDetectionLinePoint(new Segment(refPosition.Coordinates, pos), obstacles));
}
return(positions);
}
public void TestAnglePositionEqual()
{
AnglePosition a1, a2;
a1 = new AnglePosition(90);
a2 = new AnglePosition(90);
Assert.AreEqual(a1, a2);
a1 = new AnglePosition(0);
a2 = new AnglePosition(0);
Assert.AreEqual(a1, a2);
a1 = new AnglePosition(-10);
a2 = new AnglePosition(350);
Assert.AreEqual(a1, a2);
a1 = new AnglePosition(0);
a2 = new AnglePosition(360);
Assert.AreEqual(a1, a2);
a1 = new AnglePosition(90);
a2 = new AnglePosition(-270);
Assert.AreEqual(a1, a2);
}
public void Run( )
{
IEnumerable <Type> moduleTypes = typeof(Application).Assembly.GetTypes( ).
Where(type => type.GetCustomAttributes(typeof(ModuleAttribute), false).Any( ));
List <IModule> modulesToPrepare =
moduleTypes.Select(type => ( IModule )Activator.CreateInstance(type)).ToList( );
List <IModule> preparedModules = new List <IModule> (modulesToPrepare.Count);
int currentCircleLoadModCount;
do
{
currentCircleLoadModCount = 0;
List <IModule> canPrepareModules = modulesToPrepare.Where(mod => mod.Dependencies.All(dependency
=> preparedModules.Any(loadedMod =>
loadedMod.GetType( ).Name == dependency))).ToList( );
foreach (IModule mod in canPrepareModules)
{
currentCircleLoadModCount++;
modulesToPrepare.Remove(mod);
mod.Prepare(Configuration);
Logger.LogInformation($"Module {mod . GetType ( ) . Name} Prepared");
preparedModules.Add(mod);
}
}while (currentCircleLoadModCount != 0);
if (modulesToPrepare.Any( ))
{
throw new Exception("Some modules failed to load", null);
}
Random rand = new Random( );
DateTime startTime = DateTime.Now;
AnglePosition lastAngle = new AnglePosition( );
AnglePosition targetAngle = new AnglePosition(
System.Math.Sin(Angle.FromDegree((DateTime.Now - startTime).TotalSeconds * 120).
Radius) * 80,
0);
while (true)
{
Thread.Sleep(20);
SendDatagram target = null;
lastAngle = targetAngle;
targetAngle = new AnglePosition(
System.Math.Sin(Angle.FromDegree((DateTime.Now - startTime).TotalSeconds * 120).
Radius) * 80,
0);
switch (( BinaryDatagramType )rand.Next(2 + 1))
{
case BinaryDatagramType.TargetPosition:
{
target = new TargetPositionDatagram(new Point3f(
( float )System.Math.Sin(targetAngle.XYaw.Radius),
0,
( float )System.Math.Cos(targetAngle.XYaw.Radius)));
break;
}
case BinaryDatagramType.TargetAngle:
{
target = new TargetAngleDatagram(targetAngle);
break;
}
case BinaryDatagramType.TargetDeltaAngle:
{
target = new TargetDeltaAngleDatagram(targetAngle - lastAngle);
break;
}
}
if (target != null)
{
CommunicateModule.Current.SerialManager.SendDatagram(target);
}
}
//List <VideoCapture> videoCaptures = new List <VideoCapture> ( ) ;
//for ( int device = 0 ; device < 10 ; device++ )
//{
// VideoCapture cap = new VideoCapture ( device ) ;
// if ( cap . IsOpened ( ) )
// {
// videoCaptures . Add ( cap ) ;
// }
//}
//CvCamera leftCamera = new CvCamera ( videoCaptures . First ( ) ) ;
//CvCamera rightCamera = new CvCamera ( videoCaptures . Last ( ) ) ;
//using ( Window leftWindow = new Window ( "Left" ) )
//{
// using ( Window rightWindow = new Window ( "Right" ) )
// {
// while ( true )
// {
// using ( Mat leftImage = leftCamera . Read ( ) )
// {
// using ( Mat rightImage = rightCamera . Read ( ) )
// {
// leftWindow . ShowImage ( leftImage ) ;
// rightWindow . ShowImage ( rightImage ) ;
// Cv2 . WaitKey ( 20 ) ;
// }
// }
// }
// //GC . Collect ( 2 , GCCollectionMode . Forced , true ) ;
// }
// //CvBinocularCamera camera = new CvBinocularCamera(leftCamera, rightCamera);
// //camera.Open(0, 1,new Size(640, 480));
// //List < Tuple < String ^, String ^>^>^ files = gcnew List < Tuple < String ^, String ^>^> ();
// //vector<tuple<cv::Mat, cv::Mat>> images;
// //for ( int i = 1 ; i < 15 ; i++ )
// //{
// //}
//}
}
public TargetAngleDatagram(AnglePosition position)
{
Position = position;
}
public GimbalTargetDatagram(byte [] binarySource) : base(binarySource)
{
Target = new AnglePosition(BitConverter.ToSingle(binarySource, 0),
BitConverter.ToSingle(binarySource, 4));
}
public GimbalTargetDatagram(XElement xmlSource) : base(xmlSource)
{
Target = new AnglePosition(ReadNecessaryValue <float> (xmlSource, nameof(Target.XYaw)),
ReadNecessaryValue <float> (xmlSource, nameof(Target.XYaw)));
}
/// <summary>
/// Convertit la trajectoire en suite d'actions de déplacement à effectuer pour suivre la trajectoire
/// </summary>
/// <returns>Liste des déplacements correspondant</returns>
public List <ITimeableAction> ConvertToActions(Robot robot)
{
List <ITimeableAction> actions = new List <ITimeableAction>();
AnglePosition angle = _startAngle;
for (int i = 0; i < Points.Count - 1; i++)
{
RealPoint c1 = new RealPoint(Points[i].X, Points[i].Y);
RealPoint c2 = new RealPoint(Points[i + 1].X, Points[i + 1].Y);
Position p = new Position(angle, c1);
Direction traj = Maths.GetDirection(p, c2);
// Désactivation 2019 de la possibilité de faire des marches arrière pour conserver le LDIAR d'évitement dans le sens de déplacement du robot
bool canReverse = false;
// Teste si il est plus rapide (moins d'angle à tourner) de se déplacer en marche arrière avant la fin
bool inverse = false;
if (canReverse)
{
if (i < _points.Count - 2)
{
inverse = Math.Abs(traj.angle) > 90;
}
else
{
// On cherche à minimiser le tout dernier angle quand on fait l'avant dernier
AnglePosition finalAngle = angle - traj.angle;
inverse = Math.Abs(finalAngle - _endAngle) > 90;
}
if (inverse)
{
traj.angle = new AngleDelta(traj.angle - 180);
}
}
traj.angle.Modulo();
if (traj.angle < 0)
{
actions.Add(new ActionPivot(robot, -traj.angle, SensGD.Droite));
angle -= traj.angle;
}
else if (traj.angle > 0)
{
actions.Add(new ActionPivot(robot, traj.angle, SensGD.Gauche));
angle -= traj.angle;
}
if (inverse)
{
actions.Add(new ActionRecule(robot, (int)(traj.distance)));
}
else
{
actions.Add(new ActionAvance(robot, (int)(traj.distance)));
}
}
AngleDelta diff = angle - _endAngle;
if (Math.Abs(diff) > 0.2) // Angle minimal en dessous duquel on considère qu'il n'y a pas besoin d'effectuer le pivot
{
if (diff < 0)
{
actions.Add(new ActionPivot(robot, -diff, SensGD.Droite));
}
else
{
actions.Add(new ActionPivot(robot, diff, SensGD.Gauche));
}
}
return(actions);
}
protected void OnNewMeasure(List <PepperlPoint> measure, AnglePosition startAngle, AngleDelta resolution)
{
NewMeasure?.Invoke(_currentMeasure, startAngle, resolution);
_currentMeasure = null;
}
public GimbalPositionDatagram(XElement xmlSource) : base(xmlSource)
{
Position = new AnglePosition(ReadNecessaryValue <float> (xmlSource, nameof(Position.XYaw)),
ReadNecessaryValue <float> (xmlSource, nameof(Position.XYaw)));
}
public void SetDeltaAngle(AnglePosition targetDelta)
{
CommunicateModule.Current.SerialManager.SendDatagram(new TargetDeltaAngleDatagram(targetDelta));
}
public TimeSpan ExpectedMoveTime(AnglePosition target)
{
return(TimeSpan.Zero);
}
