Skip to content
Snippets Groups Projects
Commit 15b282a9 authored by Bernhard Schiffer's avatar Bernhard Schiffer
Browse files

refactoring debug painting methods

parent 91b6ab5b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Robotcontroller/robotcontroller/NativeNav.java
+ 39
− 116
View file @ 15b282a9
package robotcontroller;
import basics.math.Geom;
import basics.points.Point;
import basics.points.PointCloud2D;
import basics.points.PointCloudCreator2D;
import basics.points.util.LinkedPoint;
import robotcontroller.visualGraph.AStar;
import robotcontroller.visualGraph.Graph;
import robotcontroller.visualGraph.RoutingGraph;
import robotinterface.Robot;
import robotinterface.RobotController;
import robotinterface.RobotGeomUtil;
......@@ -30,12 +23,6 @@ import robotlib.traj.seq.TrajectorySequence;
import robotlib.worldmodel.ObstacleContainer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.stream.Collectors;
import static robotcontroller.visualGraph.util.*;
/**
* Demo controller to integrate own navigation approaches. In the current version the navigator only returns a demo route.
......@@ -80,8 +67,7 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
System.out.println("----------------------TEST-----------------------");
}
@Override
@Override
public String getDescription() {
return "Robot controller "+getClass().getName()+":\n"+
"Demo controller to integrate own navigation approaches"+
......@@ -91,55 +77,52 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
" targetAngle: target angle in degree";
}
@Override
@Override
public boolean requiresConfiguration() {
return true;
}
private void instantiateMotionPlanning() {
double[][] routingObstacles=ObstacleContainer.getObstactles2D(obstaclesLidar);
Pos2PosRouting p2p=new NativeNavP2PRouting(posXMSS, posYMSS, targetX, targetY, routingObstacles);
driver=new TrajectorySequenceDriver(
30, // rotateSpeed,
20, // travelSpeedArc,
20, // travelSpeedArcFast,
5, // travelSpeedArcBack,
50, // minFastArcRadius,
50, // minFastArcLength,
20, // travelSpeedLinear,
20, // travelSpeedLinearFast,
10, // travelSpeedLinearBack,
50 // minFastLinearLength
);
TrajectoryPlanner trajectoryPlannerWithBackdriving=new LongrangePlanner(
LongrangeProfile.createBestPracticeProfile()
.setPlanningFlagsWithTargetAngle(Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL | Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL, Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL)
.setPlanningFlagsWithoutTargetAngle(Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL | Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL, Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITHOUT_TARGET_ANGLE_WO_CL)
.setSplitDist(TRAJECTORY_PLANNING_SPLIT_DIST)
.setArcarcProfile(new double[]{1.01d,3.0d,5.0d},new double[]{1.5d,1.0d,1.0d})
.setIntArcsProfile(new double[]{1.01d,3.0d,5.0d},new double[]{1.5d,1.0d,1.0d})
.setWingArcProfile(new double[]{1.01d,4.0d,8.0d},new double[]{1.5d,1.0d,1.0d})
.setSnakeProfile(new double[]{1.01d,1.7d},new double[]{1.5d,1.0d})
.setDubinsArcsProfile(new double[]{1.01d,1.5d,3.0d},new double[]{1.5d,1.3d,1.0d})
.setTurninplaceData(150,2) // constantTurnInPlaceCosts,relativeTurnInPlaceCosts
.setBackdrivingData(100,5) // constantBackwardCosts,relativeBackwardCosts
.setChangeDrivingDirectionData(50, // changeDrivingDirectionCosts
Robot.headingDistance*3.0d) // changeDrivingDirectionMinObstacleDist
.setAlgoFlags(LongrangePlanner.FLAGS_TRAJ_ALGO_ANGLE_ALL_INCLUDING_REVERSE | LongrangePlanner.FLAGS_TRAJ_ALGO_VITERBI)
.setName("Longrange(WITH back)"),
null
);
30, // rotateSpeed,
20, // travelSpeedArc,
20, // travelSpeedArcFast,
5, // travelSpeedArcBack,
50, // minFastArcRadius,
50, // minFastArcLength,
20, // travelSpeedLinear,
20, // travelSpeedLinearFast,
10, // travelSpeedLinearBack,
50 // minFastLinearLength
);
TrajectoryPlanner trajectoryPlannerWithBackdriving=new LongrangePlanner(
LongrangeProfile.createBestPracticeProfile()
.setPlanningFlagsWithTargetAngle(Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL | Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL, Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL)
.setPlanningFlagsWithoutTargetAngle(Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL | Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITH_TARGET_ANGLE_WO_CL, Maneuver.FLAGS_ALLOW_BACKDRIVING | Maneuver.FLAGS_ARCS_UPTO_360_DEGREES | Maneuver.FLAGS_ALL_WITHOUT_TARGET_ANGLE_WO_CL)
.setSplitDist(TRAJECTORY_PLANNING_SPLIT_DIST)
.setArcarcProfile(new double[]{1.01d,3.0d,5.0d},new double[]{1.5d,1.0d,1.0d})
.setIntArcsProfile(new double[]{1.01d,3.0d,5.0d},new double[]{1.5d,1.0d,1.0d})
.setWingArcProfile(new double[]{1.01d,4.0d,8.0d},new double[]{1.5d,1.0d,1.0d})
.setSnakeProfile(new double[]{1.01d,1.7d},new double[]{1.5d,1.0d})
.setDubinsArcsProfile(new double[]{1.01d,1.5d,3.0d},new double[]{1.5d,1.3d,1.0d})
.setTurninplaceData(150,2) // constantTurnInPlaceCosts,relativeTurnInPlaceCosts
.setBackdrivingData(100,5) // constantBackwardCosts,relativeBackwardCosts
.setChangeDrivingDirectionData(50, // changeDrivingDirectionCosts
Robot.headingDistance*3.0d) // changeDrivingDirectionMinObstacleDist
.setAlgoFlags(LongrangePlanner.FLAGS_TRAJ_ALGO_ANGLE_ALL_INCLUDING_REVERSE | LongrangePlanner.FLAGS_TRAJ_ALGO_VITERBI)
.setName("Longrange(WITH back)"),
null
);
navTraj=new NavTrajSplitPlanning(p2p,trajectoryPlannerWithBackdriving);
driver.setTrajectoryEvaluator(navTraj);
}
@Override
@Override
public void configure(String params) throws IllegalArgumentException {
String[] split=params.split(";");
......@@ -182,57 +165,6 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
ovl.paint();
}
private void paintOpenedList(Collection<? extends Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("OpendList");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 5, 100, 100, 100, 255);
}
ovl.paint();
}
private void paintClosedList(Collection<? extends Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("ClosedList");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 5, 10, 10, 10, 255);
}
ovl.paint();
}
private void paintBufferPoints(List<Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Buffer Points");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 2, 0, 255, 0, 255);
}
ovl.paint();
}
private <T extends Point> void paintGraph(Graph<T> graph) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Visibility Graph");
ovl.clear();
for (T start : graph.getNodes()) {
for (T end : graph.getLinkedNodes(start)) {
ovl.drawLine(start.getX(), start.getY(), end.getX(), end.getY(), 0, 0, 255, 10);
}
}
ovl.paint();
}
private void paintRoute(List<Point> route) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Route");
ovl.clear();
Point lastPoint = route.get(0);
ovl.fillCircle(lastPoint.getX(), lastPoint.getY(), 2, 255, 0, 0, 255);
for (Point point : route.subList(1, route.size())) {
ovl.fillCircle(point.getX(), point.getY(), 2, 255, 0, 0, 255);
ovl.drawLine(point.getX(), point.getY(), lastPoint.getX(), lastPoint.getY(), 255, 0, 0, 150);
lastPoint = point;
}
ovl.paint();
}
public void run() throws Exception {
if (Robot.lidarSubsystem==null) {
......@@ -240,6 +172,8 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
return;
}
obstaclesLidar=ObstacleContainer.fromLidarObstacles(Robot.lidarSubsystem.getAllObstacles());
paintObstacles();
instantiateMotionPlanning();
Robot.lidarSubsystem.startup();
......@@ -252,23 +186,15 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
Time.sleep(100);
}
Robot.debugOut.println("Compute route...");
TrajectorySequence firstRouteTraj=computeRouteAndTrajectories(posXMSS,posYMSS,posAngMSS);
if (firstRouteTraj==null) return;
driver.drive(firstRouteTraj);
while (isRunning() && !targetReached()) {
Time.sleep(100);
double[] deviation=driver.currentDeviation();
if (deviation!=null && (deviation[0]>=DRIVER_MAX_POS_DEVIATION || deviation[1]>=DRIVER_MAX_ANG_DEVIATION)) {
Robot.debugOut.println("Deviation of position and planned trajectory too large (pos: "+Math.round(deviation[0])+"cm, "+Math.round(deviation[1]*180.0d/Math.PI)+"\u00b0)");
......@@ -296,8 +222,7 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
}
}
@Override
@Override
public void pause() throws Exception {
if (Robot.lidarSubsystem==null) {
Robot.debugOut.println("This Robot Controller requires a Lidar Subsystem");
......@@ -309,9 +234,7 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
driver.halt(Driver.HALT_REASON_CONTROLLER_PAUSE);
}
@Override
@Override
public void stop() throws Exception {
if (Robot.lidarSubsystem==null) {
Robot.debugOut.println("This Robot Controller requires a Lidar Subsystem");
......@@ -377,14 +300,14 @@ public class NativeNav extends RobotController implements MotionSubsystemListene
}
@Override
@Override
public void mssResponse(ArrayList<String> messages,int responseType) throws Exception {
if (MotionSubsystemListener.isFailureResponse(responseType))
Robot.debugOut.println("Failure response "+messages.get(0));
}
@Override
@Override
public void mssAsyncMessages(ArrayList<String> messages,AsyncMotionMessageBundle bundle) throws Exception {
if (bundle.containsPos()) {
......
Robotcontroller/robotcontroller/NativeNavP2PRouting.java
+ 69
− 35
View file @ 15b282a9
package robotcontroller;
import basics.math.Geom;
import basics.points.Point;
import basics.points.PointCloud2D;
import basics.points.PointCloudCreator2D;
import basics.points.util.LinkedPoint;
import robotcontroller.visualGraph.AStar;
import robotcontroller.visualGraph.Graph;
import robotcontroller.visualGraph.RoutingGraph;
import robotinterface.Robot;
import robotinterface.debug.DebugPainterOverlay;
import robotlib.nav.Pos2PosRouting;
import robotlib.traj.seq.TrajectorySequence;
import robotlib.traj.TrajectoryEvaluatorProfile;
import robotlib.worldmodel.ObstacleContainer;
import robotlib.traj.seq.TrajectorySequence;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.stream.Collectors;
......@@ -33,7 +33,6 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
// Die echte Routenplanung muss natürlich eine eigene Route finden
private static double[][] ROUTE;
public NativeNavP2PRouting(double fromX,double fromY,double toX,double toY,double[][] obstacles) {
ROUTE = planRoute(fromX, fromY, toX, toY, obstacles);
}
......@@ -42,9 +41,6 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
Point startPosition = new Point(fromX, fromY);
Point endPosition = new Point(toX, toY);
//paintObstacles();
//double[][] obstacles = ObstacleContainer.getObstactles2D(obstaclesLidar);
List<Point> obstaclePoints = Arrays.stream(obstacles)
.map(p -> new Point(p[0], p[1]))
.collect(Collectors.toList());
......@@ -61,34 +57,28 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
List<LinkedPoint> vg = generateVisibilityGraph(bufferPoints, obstacleCloud, 33.0);
RoutingGraph routingGraph = new RoutingGraph(vg);
//paintGraph(routingGraph);
//paintBufferPoints(bufferPoints);
paintGraph(routingGraph);
paintBufferPoints(bufferPoints);
AStar aStar = new AStar();
List<Point> route = null;
try {
route = aStar.computeRoute(routingGraph, startPosition, endPosition);
//paintRoute(route);
paintOpenedList(aStar.getOpen());
paintClosedList(aStar.getClosed());
} catch (Exception e) {
throw new RuntimeException(e);
}
//paintRoute(calculatedRoute);
//paintOpenedList(aStar.getOpen());
//paintClosedList(aStar.getClosed());
double[][] routePoints = route.stream()
.peek(p -> System.out.println("point: " + p.getX() + "/"+p.getY()))
return route.stream()
.map(p -> new double[]{p.getX(), p.getY()})
.toArray(value -> new double[value][2]);
return routePoints;
}
@Override
// Fake-Routing: es wird einfach nur die Liste oben durchwandert und die Teilliste bis zum Ziel zurückgegeben.
// Ein echtes Routingverfahren muss hier eine echt berechnete Liste von Zwischenschritten zurückgeben
@Override
// Fake-Routing: es wird einfach nur die Liste oben durchwandert und die Teilliste bis zum Ziel zurückgegeben.
// Ein echtes Routingverfahren muss hier eine echt berechnete Liste von Zwischenschritten zurückgeben
public double[][] getRoute(double fromX,double fromY,double toX,double toY,double[][] obstacles) {
// Suche den Linienabschnitt aus ROUTE, zu dem fromX,fromY am nähesten ist
......@@ -111,14 +101,12 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
return result;
}
@Override
@Override
public boolean hasRouted() {
return true;
}
@Override
@Override
public boolean addObstacles(double[][] obstacles) {
boolean result=true;
......@@ -128,31 +116,77 @@ public class NativeNavP2PRouting extends Pos2PosRouting {
return result;
}
@Override
@Override
public boolean addObstacle(double x, double y) {
// Hindernis zum eigenen Welt-Modell hinzufügen
return true; // Hinzufügen erfolgreich
}
@Override
@Override
public void paintInternalStructure(DebugPainterOverlay ovl,int red, int green, int blue, int alpha) {
// Was malen, wenn es was Interessantes gibt
}
@Override
@Override
public double evaluateTrajectorySequence(TrajectorySequence trajSeq,
TrajectoryEvaluatorProfile trajectoryEvaluatorProfile) {
// Einfache Variante: nur Geometrie der Bahn interessiert. Achtung: kein Kollisionstest da noch kein Weltmodell vorhanden
return trajSeq.evaluateWithoutObstacles(trajectoryEvaluatorProfile);
}
@Override
@Override
public double[] trajectoryCollision(TrajectorySequence trajSeq) {
return null; // Wenn es einmal ein eigenes Welt-Modell gibt, checken, wann es die erste Kollision gibt.
}
private void paintOpenedList(Collection<? extends Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("OpendList");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 5, 100, 100, 100, 255);
}
ovl.paint();
}
private void paintClosedList(Collection<? extends Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("ClosedList");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 5, 10, 10, 10, 255);
}
ovl.paint();
}
private void paintBufferPoints(List<Point> points) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Buffer Points");
ovl.clear();
for (Point point : points) {
ovl.fillCircle(point.getX(), point.getY(), 2, 0, 255, 0, 255);
}
ovl.paint();
}
private <T extends Point> void paintGraph(Graph<T> graph) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Visibility Graph");
ovl.clear();
for (T start : graph.getNodes()) {
for (T end : graph.getLinkedNodes(start)) {
ovl.drawLine(start.getX(), start.getY(), end.getX(), end.getY(), 0, 0, 255, 10);
}
}
ovl.paint();
}
private void paintRoute(List<Point> route) {
DebugPainterOverlay ovl = Robot.debugPainter.getOverlay("Route");
ovl.clear();
Point lastPoint = route.get(0);
ovl.fillCircle(lastPoint.getX(), lastPoint.getY(), 2, 255, 0, 0, 255);
for (Point point : route.subList(1, route.size())) {
ovl.fillCircle(point.getX(), point.getY(), 2, 255, 0, 0, 255);
ovl.drawLine(point.getX(), point.getY(), lastPoint.getX(), lastPoint.getY(), 255, 0, 0, 150);
lastPoint = point;
}
ovl.paint();
}
}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment