public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
{
pose = null;
this.useHokuyo = useHokuyo;
this.sickPose = sickPose;
sickDCM = Matrix4.FromPose(this.sickPose);
if (this.useHokuyo)
{
this.hokuyoPose = hokuyoPose;
hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
}
sickHistory = new List<Vector2[]>();
hokuyoHistory = new List<Vector2[]>();
this.angularResolution = angularResolution;
numBuckets = (int) Math.Round(360 / this.angularResolution);
trapped = false;
kThreshold = (int) Math.Round(160 / this.angularResolution);
rHistogram = new Double[numBuckets];
}
/// <summary>
/// Create a LocalMapRequestMessage that is sent from robots to the CentralSensorProcessor for a updated local map
/// </summary>
/// <param name="robotID">robotID</param>
/// <param name="currentPose">current position</param>
/// <param name="extentX">x-size of the local occupancy map</param>
/// <param name="extentY">y-size of the local occupancy map </param>
public LocalMapRequestMessage(int robotID, RobotPose currentPose, double extentX, double extentY)
{
this.robotID = robotID;
this.currentPose = currentPose;
this.extentX = extentX;
this.extentY = extentY;
}
static void poseClient_MsgReceived(object sender, MsgReceivedEventArgs<RobotPoseMessage> e)
{
if (e.message == null) return;
pose = e.message.Pose;
protoPose.x = pose.x;
protoPose.y = pose.y;
protoPose.z = pose.z;
protoPose.yaw = pose.yaw;
protoPose.pitch = pose.pitch;
protoPose.roll = pose.roll;
protoPose.timeStamp = pose.TimeStamp;
protoPoseServer.SendUnreliably(protoPose);
// Clear the screen
Console.Clear();
Console.SetCursorPosition(Console.CursorLeft, Console.CursorTop);
Console.WriteLine("Robot pose:");
Console.WriteLine("X:\t" + pose.x);
Console.WriteLine("Y:\t" + pose.y);
Console.WriteLine("Z:\t" + pose.z);
Console.WriteLine("yaw:\t" + pose.yaw);
Console.WriteLine("pitch:\t" + pose.pitch);
Console.WriteLine("roll:\t" + pose.roll);
Console.WriteLine("timestamp:\t" + pose.timestamp);
}
public ViconPoseProvider(String ip, Int32 port, String subject, String segment)
{
this.ip = IPAddress.Parse(ip);
this.port = port;
this.subject = subject;
this.segment = segment;
wantedChannelNames = new List<string>(6);
pose = new RobotPose();
vicon = new ViconTarsus(ip, port);
vicon.Start();
vicon.DataReceived += new EventHandler<IViconFrameEventArgs>(vicon_DataReceived);
wantedChannelNames.Add(subject + ":" + segment + " <t-X>");
wantedChannelNames.Add(subject + ":" + segment + " <t-Y>");
wantedChannelNames.Add(subject + ":" + segment + " <t-Z>");
wantedChannelNames.Add(subject + ":" + segment + " <a-X>");
wantedChannelNames.Add(subject + ":" + segment + " <a-Y>");
wantedChannelNames.Add(subject + ":" + segment + " <a-Z>");
wantedChannelNumbers = vicon.GetChannelNumbers(wantedChannelNames);
vicon.SetStreamingChannels(wantedChannelNumbers);
vicon.SetStreaming(true);
}
public RRTPlannerImpl(RobotTwoWheelStateProvider stateProvider)
{
rrt = new RRTPlannerControl();
obstacles = new List<Polygon>();
currentPose = new RobotPose();
waypoints = new PointPath();
this.stateProvider = stateProvider;
}
public RobotTwoWheelState(RobotTwoWheelState toCopy, RobotTwoWheelCommand command)
{
//deep copy the class
this.pose = new RobotPose(toCopy.pose);
this.rightWheel = new RobotWheelModel(toCopy.rightWheel);
this.leftWheel = new RobotWheelModel(toCopy.leftWheel);
this.command = command;
}
public void AddPose(RobotPose p)
{
lock (poses)
{
poses.Add(p);
if (poses.Count > maxPoints)
poses.RemoveAt(0);
}
}
public static RobotPose operator +(RobotPose left, RobotPose right)
{
RobotPose toReturn = new RobotPose();
toReturn.x = left.x + right.x;
toReturn.y = left.y + right.y;
toReturn.z = left.z + right.z;
toReturn.roll = left.roll + right.roll;
toReturn.pitch = left.pitch + right.pitch;
toReturn.yaw = left.yaw + right.yaw;
toReturn.timestamp = (left.timestamp + right.timestamp) / 2;
return toReturn;
}
public static RobotPose operator -(RobotPose left, RobotPose right)
{
RobotPose toReturn = new RobotPose();
toReturn.x = left.x - right.x;
toReturn.y = left.y - right.y;
toReturn.z = left.z - right.z;
toReturn.roll = left.roll - right.roll;
toReturn.pitch = left.pitch - right.pitch;
toReturn.yaw = left.yaw - right.yaw;
toReturn.timestamp = (left.timestamp + right.timestamp) / 2;
return(toReturn);
}
public RobotPose ToRobotPose()
{
RobotPose pose = new RobotPose(x, y, z, yaw, pitch, roll, timestamp);
pose.covariance[0, 0] = Covariance[0, 0];
pose.covariance[0, 1] = Covariance[0, 1];
pose.covariance[0, 2] = Covariance[0, 2];
pose.covariance[1, 0] = Covariance[1, 0];
pose.covariance[1, 1] = Covariance[1, 1];
pose.covariance[1, 2] = Covariance[1, 2];
pose.covariance[2, 0] = Covariance[2, 0];
pose.covariance[2, 1] = Covariance[2, 1];
pose.covariance[2, 2] = Covariance[2, 2];
return(pose);
}
PathToRobotRenderer runningPath; // the path that the robot is currently running
#endregion Fields
#region Constructors
public RobotKeypoint(int id)
{
color = Color.Black;
this.id = id;
name = "Robot " + id;
runningPath = new PathToRobotRenderer();
commandedPath = new PathToRobotRenderer();
pose = new RobotPose();
origPlygn = new Polygon();
origPlygn.Add(new Vector2(.25, .36));
origPlygn.Add(new Vector2(.25, -.36));
origPlygn.Add(new Vector2(-.25, -.36));
origPlygn.Add(new Vector2(-.25, .36));
bodyPlygn = new Polygon(origPlygn.points);
}
public RobotState()
{
RobotID = 0;
Mode = RobotMode.Starting;
SubMode = RobotSubMode.None;
Pose = new RobotPose();
rPose = new RobotPose();
PoseWatchdogFail = true;
PlannerFail = false;
InObstacle = false;
WpInObstacle = false;
IsLockedOn = false;
NeutralizeStatus = RobotNeutralizeStatus.None;
Waypoints = null;
AngleWhenDone = 0;
TrackedPath = null;
SparsePath = null;
Cmd = new RobotTwoWheelCommand(0, 0);
CmdTime = DateTime.Now;
WaypointsCompleted = 0;
}
public RobotState()
{
RobotID = 0;
Mode = RobotMode.Starting;
SubMode = RobotSubMode.None;
Pose = new RobotPose();
rPose = new RobotPose();
PoseWatchdogFail = true;
PlannerFail = false;
InObstacle = false;
WpInObstacle = false;
IsLockedOn = false;
NeutralizeStatus = RobotNeutralizeStatus.None;
Waypoints = null;
AngleWhenDone = 0;
TrackedPath = null;
SparsePath = null;
Cmd = new RobotTwoWheelCommand(0, 0);
CmdTime = DateTime.Now;
WaypointsCompleted = 0;
}
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;
}
/// <summary>
/// Find targets above grounds and return in global and robot coordinate
/// </summary>
/// <param name="robotPose"></param>
/// <param name="laserScan"></param>
/// <param name="localPoints"></param>
/// <param name="globalPoints"></param>
public void DetectTarget(RobotPose robotPose, ILidarScan<ILidar2DPoint> laserScan, out List<Vector3> laserPoints, out List<Vector3> localPoints, bool lidarUpsideDown)
{
// initialization
localPoints = new List<Vector3>();
laserPoints = new List<Vector3>();
for (int i = 0; i < laserScan.Points.Count; i++)
{
Vector4 localPt = laserToRobot * laserScan.Points[i].RThetaPoint.ToVector4();
Vector4 newLocalPt;
if (lidarUpsideDown)
newLocalPt = new Vector4(-localPt.Y, localPt.X, localPt.Z, localPt.W);
else
newLocalPt = new Vector4(localPt.Y, localPt.X, localPt.Z, localPt.W);
localPoints.Add(new Vector3(newLocalPt.X, newLocalPt.Y, newLocalPt.Z));
if (lidarUpsideDown)
laserPoints.Add(new Vector3(laserScan.Points[i].RThetaPoint.R, laserScan.Points[i].RThetaPoint.theta, laserScan.Points[i].RThetaPoint.thetaDeg));
else
laserPoints.Add(new Vector3(laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.R,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.theta,
laserScan.Points[laserScan.Points.Count - i - 1].RThetaPoint.thetaDeg));
}
}
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = scan.Points.Count;
this.robotPose = robotPose;
this.startIdx = 0;
this.endIdx = scan.Points.Count - 1;
}
}
/// <summary>
/// Return a 2D vector point in EN framework given two robot pose and pixel reading
/// </summary>
/// <param name="r1">Robot1 pose</param>
/// <param name="r2">Robot2 pose</param>
/// <param name="pixelR1">pixel reading from robot1</param>
/// <param name="pixelR2">pixel reading from robot2</param>
/// <param name="focalLength">focal length of the camera you're using</param>
/// <returns>2D point of target for initialization</returns>
public static Vector2 ComputeInitialPoint(RobotPose r1, RobotPose r2, Vector2 pixelR1, Vector2 pixelR2, double centerPixX, double focalLength)
{
double u1 = pixelR1.X; double u2 = pixelR2.X;
double gAngle1 = r1.yaw - Math.Atan2(u1 - centerPixX, focalLength);
double gAngle2 = r2.yaw - Math.Atan2(u2 - centerPixX, focalLength);
double xGoal = ((r2.y - r1.y) - Math.Tan(gAngle2) * r2.x + Math.Tan(gAngle1) * r1.x) / (Math.Tan(gAngle1) - Math.Tan(gAngle2));
double yGoal = Math.Tan(gAngle1) * (xGoal - r1.x) + r1.y;
return new Vector2(xGoal, yGoal);
}
public LidarPolyExtractor(double threshold, RobotPose curRobotPose, SensorPose curLidarToBody)
{
this.curLidarToBody = curLidarToBody;
this.curRobotPose = curRobotPose;
this.threshold = threshold;
}
public void SetScan(ILidarScan<ILidar2DPoint> scan, RobotPose robotPose, int startIdx, int endIdx)
{
lock (this.drawLock)
{
this.scan = scan;
this.time = scan.Timestamp;
this.numPoints = endIdx - startIdx + 1;
this.robotPose = robotPose;
this.startIdx = startIdx;
this.endIdx = endIdx;
}
}
/// <summary>
/// Method to fill object variables
/// </summary>
/// <param name="lidarscandata">lidar scan data in the form of a List of Vector2s</param>
public void RunDetector(ILidarScan<ILidar2DPoint> LidarScan, RobotPose pose)
{
//ScanToTextFile(LidarScan); //export scan data to analyze in matlab
List<Vector2> lidarscandata = new List<Vector2>(LidarScan.Points.Count);
for (int ii = 0; ii < LidarScan.Points.Count; ii++)
{
if (LidarScan.Points[ii].RThetaPoint.R < rangetoconsider)
lidarscandata.Add(LidarScan.Points[ii].RThetaPoint.ToVector2(isSickUpsideDown));
}
if (isSickUpsideDown)
lidarscandata.Reverse();
polygons = ClusterScanData(lidarscandata);
//PolygonsToTextFile(); //export polygon data to analyze in matlab
AllGaps = FindAllGaps(polygons, pose);
CheckGaps(AllGaps);
}
public void UpdateRobotPose(RobotPose p)
{
curRobotPose = new RobotPose (p);
}
/// <summary>
/// Finds gaps between polygon objects indiscriminately.
/// </summary>
/// <param name="polys">List of polygons to find gaps between</param>
/// <returns>List of LineSegment objects representing gaps found</returns>
private List<LineSegment> FindAllGaps(List<Polygon> polys, RobotPose pose)
{
List<LineSegment> gaps = new List<LineSegment>();
for (int i = 0; i < polys.Count; i++)
{
Polygon poly1 = polys[i];
for (int j = i + 1; j < polys.Count; j++)
{
Polygon poly2 = polys[j];
LineSegment gap = poly1.ShortestLineToOther(poly2);
if (gap.Length <= ignoreabovethis)
{
Vector2 startGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P0.X - Math.Sin(pose.yaw) * gap.P0.Y + pose.x,
Math.Sin(pose.yaw) * gap.P0.X + Math.Cos(pose.yaw) * gap.P0.Y + pose.y);
Vector2 endGlobal = new Vector2(Math.Cos(pose.yaw) * gap.P1.X - Math.Sin(pose.yaw) * gap.P1.Y + pose.x,
Math.Sin(pose.yaw) * gap.P1.X + Math.Cos(pose.yaw) * gap.P1.Y + pose.y);
LineSegment gapGlobal = new LineSegment(startGlobal, endGlobal);
gaps.Add(gapGlobal);
}
}
}
return gaps;
}
public void UpdateMeanCovariance(List<Vector2> pt, List<Matrix> cov, RobotPose currentPose)
{
mean.Clear(); covariance.Clear();
if (pt.Count != cov.Count)
throw new ArgumentException("Length of List<Vector2> and List<Matrix> do not match");
else
{
for (int i = 0; i < pt.Count; i++)
{
double x = pt[i].X;// pt[i].X * Math.Cos(currentPose.yaw) - pt[i].Y * Math.Sin(currentPose.yaw) + currentPose.x;
double y = pt[i].Y;// pt[i].X * Math.Sin(currentPose.yaw) + pt[i].Y * Math.Cos(currentPose.yaw) + currentPose.y;
mean.Add(new List<Vector2>());
covariance.Add(new List<Vector2>());
// mean
//mean[i].Add(new Vector2(x - width / 2, y - width / 2));
//mean[i].Add(new Vector2(x + width / 2, y - width / 2));
//mean[i].Add(new Vector2(x + width / 2, y + width / 2));
//mean[i].Add(new Vector2(x - width / 2, y + width / 2));
mean[i].Add(new Vector2(x, y));
// covariance ellipse
Matrix ellipse = GenerateEllipse(numCovPt, x, y, cov[i], sigma);
for (int j = 0; j < ellipse.Rows; j++)
{
covariance[i].Add(new Vector2(ellipse[j, 0], ellipse[j, 1]));
}
}
}
}
public RobotPose(RobotPose toCopy)
: base(toCopy)
{
this.covariance = new Matrix(toCopy.covariance.Array);
}
/// <summary>
/// Return X, Y coordinate of a pixel given Z and focal length information
/// </summary>
/// <param name="focalLength"></param>
/// <param name="range"></param>
/// <param name="pixel"></param>
/// <returns></returns>
public static Vector2 FindPosCoord(string screenSize, double range, double pixelX, double pixelY, RobotPose robotPose, SensorPose lidarPose)
{
double focalLength = 0;
double centerPixX = 0;
double centerPixY = 0;
if (screenSize.Equals("320x240"))
{
focalLength = (384.4507 + 384.1266) / 2;
centerPixX = 160; centerPixY = 120;
}
else if (screenSize.Equals("640x480"))
{
focalLength = (763.5805 + 763.8337) / 2;
centerPixX = 320; centerPixY = 240;
}
else if (screenSize.Equals("960x240"))
{
focalLength = (345.26498 + 344.99438) / 2;
centerPixX = 960 / 2; centerPixY = 480 / 2;
}
double angle = Math.PI / 2 - Math.Atan2(pixelX - centerPixX, focalLength);
Matrix localPt = new Matrix(3, 1);
localPt[0, 0] = range * Math.Cos(angle); localPt[1, 0] = range * Math.Sin(angle);
double yaw = robotPose.yaw - Math.PI / 2; double pitch = robotPose.pitch; double roll = robotPose.roll;
Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) *
new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) *
new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll));
Matrix globalPt = R_ENU2R.Inverse * localPt;
return new Vector2(globalPt[0, 0] + robotPose.x, globalPt[1, 0] + robotPose.y);
}
public void Reset()
{
gotFirstUpdate = false;
state = new RobotPose();
}
public RobotPose(RobotPose toCopy)
: base(toCopy)
{
this.covariance = new Matrix(toCopy.covariance.Array);
}
public void UpdatePose(RobotPose pose)
{
currentPoint = new RobotPose(pose);
currentPoint.z = 0;
}
public void UpdatePose(RobotPose pose)
{
lock (locker)
{
this.currentPose = pose;
}
}
public void UpdatePose(RobotPose pose)
{
{
this.currentPoint.x = pose.x;
this.currentPoint.y = pose.y;
this.currentPoint.yaw = pose.yaw;
}
}
public void ForceState(RobotPose state)
{
gotFirstUpdate = false;
this.state = state;
}
/// <summary>
/// Update rover position and remove nodes from waypoints and planned path if the rover approaches them
/// </summary>
/// <param name="pose"></param>
public void UpdatePose(RobotPose pose)
{
lock (plannerLock)
{
List<SimpleTreeNode<Vector2>> nodesToRemove = new List<SimpleTreeNode<Vector2>>();
currentLocation = pose.ToVector2();
//Remove waypoints from the list as the rover nears them
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in userWaypoints)
{
if (currentLocation.DistanceTo(node.Value) < wayPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
userWaypoints.Remove(node);
}
//Remove nodes from the planned path as the rover approaches them
lock (pathLock)
{
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in outputNodeList)
{
if (currentLocation.DistanceTo(node.Value) < pathPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
outputNodeList.Remove(node);
}
}
}
}
/// <summary>
/// Call this so the renderer "knows" where the robots are.
/// This allows Renderables that have the "DrawVehicleRelative" property set
/// to true to be drawn correctly.
/// </summary>
/// <param name="robotID"></param>
/// <param name="pose"></param>
public void UpdateRobotPose(int robotID, RobotPose pose)
{
if (pose == null) return;
lock (robotPoseLock)
{
if (robotPoses.ContainsKey(robotID))
robotPoses[robotID] = pose;
else
{
robotPoses.Add(robotID, pose);
}
}
}
public void UpdatePose(RobotPose pose)
{
this.pose = pose;
}
