/// <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); } } } }
public void UpdatePose(RobotPose pose) { this.pose = pose; if (waypoints == null || waypoints.Count == 0) { return; } if (waypointsAchieved >= waypoints.Count) { return; } if (pose.ToVector2().DistanceTo(waypoints.ElementAt(waypointsAchieved).Coordinate) < achieveThreshold) { waypoints.ElementAt(waypointsAchieved).Achieved = true; WaypointAchieved(this, new WaypointAchievedEventArgs(waypoints[waypointsAchieved])); waypointsAchieved++; } }
private void PlanPurePursuit() { if (pathCurrentlyTracked == null) { return; } //we are really far off the path, just get on the stupid path //mark sucks and wants this behavior if (pathCurrentlyTracked.Length == 0) { goalPoint = pathCurrentlyTracked.EndPoint.pt; } else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2) { //Console.WriteLine("2"); double lookaheadRef = 0; PointOnPath pTemp = segmentCurrentlyTracked.StartPoint; goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt; } else { //Console.WriteLine("1"); //see if we can track the next segment and if so, update that... PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2()); double lookaheadRef = lookAheadDistParam; PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef); //the path lookahead point is at the end, and we cant do antyhing segmentCurrentlyTracked = lookaheadPointOnPath.segment; goalPoint = lookaheadPointOnPath.pt; } double errorX = (goalPoint.X - currentPoint.x); double errorY = (goalPoint.Y - currentPoint.y); Vector2 verr = new Vector2(errorX, errorY); //Console.WriteLine(errorX + " | " + errorY); double errorDistance = verr.Length; double currentYaw = currentPoint.yaw; double errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw); //Console.Write("neg Hyst: " + hysterisisNegative + " pos Hyst: " + hysterisisPositive + " yawError" + errorYaw * 180.0/ Math.PI+ " ||"); //the reason for this is that our angles are defined from 0 to 360 //but the PID controller expects angle to be -180 to 180 //calculate the control outputs //the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual //position double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI; double commandedVelocity = (kPPosition * errorDistance); double commandedHeading = kPHeading * errorYaw; if (Math.Abs(commandedVelocity) > capVelocity) { commandedVelocity = Math.Sign(commandedVelocity) * capVelocity; } if (Math.Abs(commandedHeading) > capHeadingDot) { commandedHeading = Math.Sign(commandedHeading) * capHeadingDot; } //if (unsignedYawErrorNormalizeToOne > .1) // find which input to use RobotTwoWheelCommand currentInput = inputList[pathCurrentlyTracked.IndexOf(segmentCurrentlyTracked)]; //commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor)); //command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading); if (pathCurrentlyTracked.EndPoint.pt.DistanceTo(currentPoint.ToVector2()) < .2) { command = new RobotTwoWheelCommand(0, 0); } else { if (currentInput.velocity < 0.3) { command = new RobotTwoWheelCommand(0.4, commandedHeading); } else { command = new RobotTwoWheelCommand(currentInput.velocity, commandedHeading); } } //if (reachedGoal) //{ // command.velocity = 0; // command.turn = 0; //} //Console.WriteLine("Current: " + currentPoint.x + " " + currentPoint.y + " " + currentPoint.yaw + " | " + errorDistance + " | " + errorYaw + " | " + commandedVelocity + " " + commandedHeading); //Console.WriteLine(); }
public IPath GetPathToGoal(IPath[] otherPaths, out IPath sparsePath, out bool inObstacle, out bool wpInObstacle) { inObstacle = false; wpInObstacle = false; sparsePath = null; if (waypoints == null) { return(null); } if (waypointsAchieved >= waypoints.Count) { return(null); } if (og == null) { return(null); } if (bloatedObstacles == null) { Console.WriteLine("Convolved obstacles fail!"); return(null); } // Add current pose position to filteredWaypoints. (FilteredWaypoints is what we will be // planning over. Waypoints is list of user given waypoints.) List <Waypoint> filteredWaypoints = new List <Waypoint>(); filteredWaypoints.Add(new Waypoint(pose.ToVector2(), true, 0)); lock (ogLock) { if (og.GetCell(pose.x, pose.y) == 255) { inObstacle = true; og.SetCell(pose.x, pose.y, 0); } } // Only add user waypoints to PathPoints if they have not been achieved yet. // If they are inside an obstacle, return a null path. foreach (Waypoint wp in waypoints) { if (!wp.Achieved) { lock (bloatedObstacleLock) { foreach (Polygon p in bloatedObstacles) { if (p.IsInside(wp.Coordinate)) { wpInObstacle = true; return(null); } } } filteredWaypoints.Add(wp); } } // Plan each segment of the filteredWaypoints individually and stitch them together. List <Waypoint> completeRawPath = new List <Waypoint>(); for (int i = 0; i < filteredWaypoints.Count - 1; i++) { bool pathOk; List <Waypoint> rawPath; lock (ogLock) { rawPath = dstar.FindPath(filteredWaypoints[i], filteredWaypoints[i + 1], og, out pathOk); } if (!pathOk) { return(null); } else { int j; // We want to only add the first path waypoint if it is the first // segment of the whole path. if (i == 0) { j = 0; } else { j = 1; } for (; j < rawPath.Count; j++) { completeRawPath.Add(rawPath.ElementAt(j)); } } } if (completeRawPath.Count > 0) { IPath bezierPath; PointPath segmentedBezierPath; lock (bloatedObstacleLock) { bezierPath = smoother.Wp2BezierPath(completeRawPath, bloatedObstacles, collapseThreshold); } sparsePath = new PointPath(completeRawPath); segmentedBezierPath = smoother.ConvertBezierPathToPointPath(bezierPath, bezierSegmentation); // We want to check our sparse path against other sparse paths to see // if there are any intersections. If there are, we want to know if the // intersections will be a problem. If it is a problem, bloat that sparse path, // add it to obstacles, and replan. for (int i = 0; i < otherPaths.Length; i++) { if (otherPaths[i] != null && otherPaths[i].Count > 0 && i + 1 != robotID) { double intersectDist = DoPathsCollide(sparsePath, otherPaths[i]); if (intersectDist != -1) { PointOnPath collisionPt; int collisionSegmentIndex; double tempDist = intersectDist - collideBackoff; tempDist = Math.Max(0, tempDist); collisionPt = segmentedBezierPath.AdvancePoint(segmentedBezierPath.StartPoint, ref tempDist); collisionSegmentIndex = segmentedBezierPath.IndexOf(collisionPt.segment); segmentedBezierPath.RemoveRange(collisionSegmentIndex, segmentedBezierPath.Count - collisionSegmentIndex); tempDist = intersectDist - collideBackoff; tempDist = Math.Max(0, tempDist); collisionPt = sparsePath.AdvancePoint(sparsePath.StartPoint, ref tempDist); collisionSegmentIndex = sparsePath.IndexOf(collisionPt.segment); sparsePath.RemoveRange(collisionSegmentIndex, sparsePath.Count - collisionSegmentIndex - 1); sparsePath[collisionSegmentIndex].End = collisionPt.pt; } } } return(segmentedBezierPath); } return(null); }
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt) { PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2()); int numpoints = (int)((path.Length) / deltadist); int n = 5; //number of additional points xr = new double[numpoints + n]; yr = new double[numpoints + n]; double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000; timestamps = new double[numpoints]; for (int i = 0; i < timestamps.Length; i++) { timestamps[i] = starttimestamp + i * dt; } for (int i = 0; i <= numpoints + 3; i++) { double d = deltadist * (i); PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d); xr[i] = lookaheadpointi.pt.X; yr[i] = lookaheadpointi.pt.Y; } double[] xrdot = new double[xr.Length - 1]; double[] yrdot = new double[xrdot.Length]; double[] xr2dot = new double[xrdot.Length - 1]; double[] yr2dot = new double[xr2dot.Length]; double[] vr = new double[xr.Length]; double[] wr = new double[xr.Length]; double[] hr = new double[xr.Length]; for (int i = 0; i < xr.Length - 1; i++) { xrdot[i] = (xr[i + 1] - xr[i]) / dt; yrdot[i] = (yr[i + 1] - yr[i]) / dt; } for (int i = 0; i < xrdot.Length - 1; i++) { xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt; yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt; } for (int i = 0; i < xrdot.Length; i++) { vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2)); hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians // unwrap headings if (i > 0) { while (hr[i] - hr[i - 1] >= Math.PI) { hr[i] -= 2 * Math.PI;; } while (hr[i] - hr[i - 1] <= -Math.PI) { hr[i] += 2 * Math.PI; } } if (i < xr2dot.Length) { wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec } } //pad the reference vectors for (int i = 1; i < n; i++) { wr[numpoints + i] = 0; vr[numpoints + i] = 0; hr[numpoints + i] = 0; } TextWriter tw = new StreamWriter("parametrization.txt"); for (int i = 0; i < timestamps.Length; i++) { tw.WriteLine("index timestamp x y vr wr hr"); tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}", i, timestamps[i], xr[i], yr[i], vr[i], wr[i], hr[i]); } tw.Close(); }
public RobotTwoWheelCommand GetCommand(double transMax, double turnMax) { lock (followerLock) { if (pathCurrentlyTracked == null) { Console.WriteLine("Null path tracked!"); return(new RobotTwoWheelCommand(0, 0)); } //we are really far off the path, just get on the stupid path //mark sucks and wants this behavior if (pathCurrentlyTracked.Length == 0) //single waypoint case { goalPoint = pathCurrentlyTracked.EndPoint.pt; startPoint = pathCurrentlyTracked.EndPoint.pt; } //else if (segmentCurrentlyTracked.DistanceOffPath(currentPoint.ToVector2()) > lookAheadDistParam / 2) //{ // //Console.WriteLine("2"); // double lookaheadRef = 0; // PointOnPath pTemp = segmentCurrentlyTracked.StartPoint; // goalPoint = pathCurrentlyTracked.AdvancePoint(pTemp, ref lookaheadRef).pt; //} else { //Console.WriteLine("1"); //see if we can track the next segment and if so, update that... PointOnPath currentPointOnPath = segmentCurrentlyTracked.ClosestPoint(currentPoint.ToVector2()); double lookaheadRef = lookAheadDistParam; PointOnPath lookaheadPointOnPath = pathCurrentlyTracked.AdvancePoint(currentPointOnPath, ref lookaheadRef); if (segmentCurrentlyTracked != lookaheadPointOnPath.segment) { segmentCurrentlyTracked = lookaheadPointOnPath.segment; } goalPoint = lookaheadPointOnPath.pt; startPoint = currentPointOnPath.pt; } // Calculate errors double errorX = (goalPoint.X - currentPoint.x); double errorY = (goalPoint.Y - currentPoint.y); Vector2 verr = new Vector2(errorX, errorY); double tangentX = (goalPoint.X - startPoint.X); double tangentY = (goalPoint.Y - startPoint.Y); Vector2 tangent = new Vector2(tangentX, tangentY); double errorDistance = currentPoint.ToVector2().DistanceTo(startPoint); double currentYaw = currentPoint.yaw; double tempCurrentYaw = currentPoint.yaw; errorYawTangent = UnwrapAndCalculateYawError(tangentX, tangentY, ref tempCurrentYaw); errorYaw = UnwrapAndCalculateYawError(errorX, errorY, ref currentYaw); double tangentEquation = (goalPoint.Y - startPoint.Y) * currentPoint.x / (goalPoint.X - startPoint.Y) - (goalPoint.Y - startPoint.Y) * goalPoint.X / (goalPoint.X - startPoint.Y) + startPoint.Y; //Console.Clear(); //Console.WriteLine("Current yaw is: " + currentYaw); if (tangentEquation < currentPoint.y) { if (Math.PI / 2 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI) { //Console.WriteLine("Above line, pointing left"); errorDistance *= -1; } //else Console.WriteLine("Above line, pointing right"); } else { if (0 <= Math.Abs(currentYaw) && Math.Abs(currentYaw) <= Math.PI / 2) { //Console.WriteLine("Below line, pointing right"); errorDistance *= -1; } //else Console.Write("Below line, pointing left"); } //if we are really close to last waypoint, make the robot just stop if ((segmentCurrentlyTracked == pathCurrentlyTracked[pathCurrentlyTracked.Count - 1]) && (PathCurrentlyTracked.EndPoint.pt - currentPoint.ToVector2()).Length < lookAheadDistParam) { command = new RobotTwoWheelCommand(0, 0); //Console.WriteLine("Acheived!"); } else { //the idea here is we want to make the velocity (which is a derivative) be proportional to the error in our actual //position double unsignedYawErrorNormalizeToOne = Math.Abs(errorYaw) / Math.PI; double velocityPercentage = Math.Abs((Math.PI - Math.Abs(errorYawTangent)) / Math.PI); double commandedVelocity = (velocityPercentage < 0.5) ? 0.0 : transMax * velocityPercentage; //double commandedHeading = 200 * errorYawTangent + 100 * errorYaw + 300 * errorDistance; double commandedHeading = 150 * errorYawTangent + 100 * errorYaw + 200 * errorDistance; if (Math.Abs(commandedVelocity) > transMax) { commandedVelocity = Math.Sign(commandedVelocity) * transMax; } if (Math.Abs(commandedHeading) > turnMax) { commandedHeading = Math.Sign(commandedHeading) * turnMax; commandedVelocity = 0.0; } /*if (unsignedYawErrorNormalizeToOne > .1) * commandedVelocity *= (1 - Math.Pow(unsignedYawErrorNormalizeToOne, velocityTurningDamingFactor));*/ command = new RobotTwoWheelCommand(commandedVelocity, commandedHeading); //Console.WriteLine(errorYaw + ", " + errorYawTangent + ", " + errorDistance); } return(command); } }