Navigator class - how to set Waypoint() coordinates?

[Orfeus]

Hello,

I have created a program in Java SE, which send coordinates and angles to the NXT by bluetooth (screen)


Everything works right if I don't use the navigator class.

Code: Select all

   for (int i = 0; i < distances.length; i ++ ) {
            robot.travel(distances[i]);         // cm
            robot.rotate(degress[i]);              // degrees
  }

field distances contains four distances which robot has to travel in centimeters. (result of point1.distance(point2));
field degrees contains four angles which robot has to rotate after he travelled the distance from previous step.
Distances and angles are counted on the client (PC part) of the program and send to the NXT, so NXT just travel pre-counted "path".

for example, field "distances" based on the screenshot will contain: (distances[0] = (int) (point1.distance(point2) * 2) / 10;)
200, 100, 200, 100

field degrees will contain:
90,90,90,90

Everything what i described until here works fine, but I have to use Navigation (Navigator class) in my project instead of counting distances and angles myself, but I'm not sure how does Navigator class (http://lejos.sourceforge.net/nxt/nxj/api/index.html) works. Can someone please put here how to set waypoints to travel rectangle 1m x 2m?
I know how to use waypoints ( navigator.addWaypoint(new Waypoint(100, 0)); ), but don't know how to set coordinates to travel rectangle of 2x1m (at the end robot should be on the spot where its travel begins).
I have tried this:

Code: Select all

        DifferentialPilot pilot = new DifferentialPilot(12.8, 8.2, Motor.B, Motor.C, true);
        Navigator navigator = new Navigator(pilot);

        navigator.addWaypoint(new Waypoint(200, 0));
        navigator.addWaypoint(new Waypoint(200, 100));
        navigator.addWaypoint(new Waypoint(0, 100));
        navigator.addWaypoint(new Waypoint(0,0));

        navigator.followPath();
        Button.ENTER.waitForPressAndRelease();

And the result you can see here: (the rectangle from ducktape on carpet is 2x1m and the robot should stay on black lines)

http://www.youtube.com/watch?v=GUM-zsys ... uYDpd_VV4=

And how to set initial position from 0,0 to for example 5,5? There were a method for this (i think) in the class TachoNavigatorsetPosition(float x, float y, float directionAngle) but the whole class seems to be gone.

Thanks for answer.

[matthias]

for me its looks a bit like two issues:
First: The initiation of the Pilot is wrong - the wheel you set is too small, so the first line the roboter drives is only half as long as it should be.
Second: The Navigator seems to work like: Travel to a point, that is relative to the nxt itself, so it does not keep the initial position.
So your waypoint should be like
200 0
0 100
-200 0
0 -100

(only assumptions!)

greetings
matthias

[roger]

The Navigator uses a PoseProvider (as mentioned in the javadocs) to track the robot position as it moves. The default PoseProvider is the OdometryPoseProvider which is created by the Navigator constructor. To set the initial position of the robot, use the setPose() method of the PosePoseProvider. By default, the initial pose is (0,0,0);
If this is confusing, you might want to look at the javadocs for Pose and OdometryPoseProvider.
Good luck,
Roger

[Mikee_ITguy]

May I also ask few stuff about the Navigator class plz??

I am trying to make a game field 40x80 centimeters rectangle and navigator to go to certain places on it. Another person on this forum has also asked similar question about it and even though I tried to help I am not sure I have provided correct information, since when I load it in my robot it gives me exception and I don't know why.

My code is:

Code: Select all

         Line [] lines = new Line[4];
         
         lines [0] = new Line(2, 2, 38, 2);   //left >> right
         lines [1] = new Line(38, 2, 38, 78);      // down >> up
         lines [2] = new Line(38, 78, 2, 78);
         lines [3] = new Line(2, 78, 2, 2);   //full rectangle

         lejos.geom.Rectangle bounds = new Rectangle(0, 0, 50, 90);

          this.myMap = new LineMap(lines, bounds);
   
         FourWayGridMesh grid = new FourWayGridMesh(myMap, 15, 10);

         AstarSearchAlgorithm alg = new AstarSearchAlgorithm();
         
         this.pf = new NodePathFinder(alg, grid);   

   PoseProvider position = new OdometryPoseProvider(robot);
                   
                   Navigator nav = new Navigator(robot, position) ;
                   System.out.println("Planning path...");
                   nav.followPath(pf.findRoute(position.getPose(), new Waypoint(20, 40)));
                   nav.waitForStop();
         

Problem is that robot will not move and throw exception after Planning path. What am I doing wrong ??
Can somebody more skilled EXPLAIN navigator class or link me to some good text / documentation, coz I don't get the API documentation at all.
Thank you all,
Mike

[roger]

Hi Mike,
I do not immediately see what is wrong with your code. Are you sure the problem is with the navigator? You might try looking at the route that is generated by the NotePathFinder to see if the problem might be there. If the output fills the robot screen, you could use RConsole to display the way point coordinates.
Good luck,
Roger

[Mikee_ITguy]

Hello Roger and thanks for answer,

I am quite positive that it is either the grid or navigator, coz I have commented out everything else now
Good point with displaying the route, I shall learn a little about this NotePathFinder and RConsole and test it during weekend to see what's what.

Thank you for a new options to try, I was truly lost with no ideas what to do.
Mike

[roger]

I assume your are still getting an exception. I should have suggested in my last post that you find out what is the exception and what method call is causing it.
You can find out how to do this in the tutorial . see
http://lejos.sourceforge.net/nxt/nxj/tutorial/ErrorHandlingAndDebugging/ErrorHandling_and_debugging.htm
Let me know what you find out.
Roger

[Mikee_ITguy]

Hi again Roger,
I was quite bussy now, so I plan to try to solve this on weekend to come, but because I was stuck for some time I was spamming everyone I was thinking that might solve it and I got some response today.
According to Paul Matthias Scholz I got it the other way around and set the lines to be obstacles, so he is basically trying to navigate from an obstacle, which could very much be the problem. As I said I didn't have much time now, so on weekend we shall see, but it seems very likely that I got the navigator the other way around )
About the debugging and error handling I did that the first time I got the error saying class 80 in navigator class, it was my first thinking step - there is an error, how do I debug it ? google: lejos debugging )

Soon I will know for sure, in the meantime thanks for trying to help.
Have a good evening,
Mike

[Mikee_ITguy]

Hi again Roger and everybody,

EDIT: Just about my previous post, the solution is never that simple, even if I remove the lines or make them take small place on the map the exception remains and robot doesn't move.

I had some time today to play with the navigator and I don't seem to get the grasp around it at all

Code: Select all

DifferentialPilot robot = new DifferentialPilot(5.5,9,Motor.A,Motor.C,false);
PoseProvider posep = new OdometryPoseProvider(robot);
RConsole.openUSB(30000);
Line [] lines = new Line[4];

// Create a map, I am even trying the lines in case something is wrong with rectangle, so they basically copy it
lines [0] = new Line(0, 0, 40, 0);   //left >> right bottom
lines [1] = new Line(40, 0, 40, 80);      // down >> up
lines [2] = new Line(40, 80, 0, 80);
lines [3] = new Line(40, 80, 0, 0);   //full rectangle
lejos.geom.Rectangle bounds = new Rectangle(0, 0, 40, 80);
LineMap myMap = new LineMap(lines, bounds);

//test if I am inside the map           
RConsole.println("I am inside the map");     
RConsole.println(String.valueOf(myMap.inside(new Point(posep.getPose().getX(), posep.getPose().getY()))));     // the result of this is true

FourWayGridMesh grid = new FourWayGridMesh(myMap, 20, 15);
AstarSearchAlgorithm alg = new AstarSearchAlgorithm();
PathFinder pf = new NodePathFinder(alg, grid);
RConsole.println("Waypoint is in the map");         
RConsole.println(String.valueOf(myMap.inside(new Point (20,20))));                       // this is also true                
Navigator nav = new Navigator(robot, posep) ;
System.out.println("Planning path...");
      
                   // HERE IS THE PROBLEM AND EXCEPTION

RConsole.println("path is:");
RConsole.println(String.valueOf(pf.findRoute(posep.getPose(), new Waypoint(20,20))));

//and so I don't get to continue to use it the navigator itself, it is the path which is a problem.                 
Button.waitForAnyPress();
try {   nav.followPath(pf.findRoute(posep.getPose(), new Waypoint(10,20)));
                  } catch (DestinationUnreachableException e) {
                     e.printStackTrace();
                     Button.waitForAnyPress();
                  }

when I try to debug the exception which I got by the nxjdebugtool: I get told it is in 91st line of NodePathFinder

Code: Select all

\workspace\Navigator>nxjdebugtool -di navi.nxd -c -m 83 89 91

The class number 83 refers to:
  lejos.robotics.navigation.DestinationUnreachableException (DestinationUnreacha
bleException.java)

The method number 89 refers to:
  lejos.robotics.pathfinding.NodePathFinder.findRoute(Llejos/robotics/navigation
/Pose;Llejos/robotics/navigation/Waypoint;)Llejos/robotics/pathfinding/Path; (No
dePathFinder.java)

PC 91 refers to:
line 83 in NodePathFinder.java


Which is a piece of code saying :

Code: Select all

      if(path == null) throw new DestinationUnreachableException();
   
      if(mesh != null) {
         mesh.removeNode(startNode);
         mesh.removeNode(goalNode);
      }
      
      return path;

So my guess is it returns NULL to the navigator, but I don't understand why or how to fix this

Any ideas anyone, please ?

Could the problem be that I am entering coordinates when the program expects Node?
If so how to fix it and how come that for the sample.pathfinding it is working ok ??

[roger]

Hi Mike,
You are making progress. We now know that your code quit, not because of a problem with Navigator, but because a "DestinationUnreachableException" was thrown by the pathfinder, presumably because it could not find a path from the initial pose to the destination. I notice that the lines that define your map connect to form a rectangle. Mathias Paul is correct. The segments in a line map represent the obstacles to be avoided. If your destination is inside closed rectangle and your initial pose is outside (for example at 0,0) then the destination is unreachable. If you leave out one of the lines, the pathFinder should be able to work for you. Good luck.
Roger
BTW, why did you chose to use the NodePathFinder and the FourWayGridMesh? The ShortestPathFinder uses the LineMap directly thus saving a few lines of code.

[Mikee_ITguy]

Good evening and THANK YOU Roger,

Yes, you are absolutely right, I was getting it all wrong from beginning and didn't try to look for errors before navigator itself, so the NodePathFinder really stuck me and I could find no answers, because I was asking wrong question ... obviously.

I tried to delete one of the lines, it didn't help though, still giving me exception error that destination is unreachable, but as you suggested I used the ShortestPathFinder instead and it is working like a charm for me, true magic

You are a livesaver, I spent a week trying to get my head around it with no success at all. Now I can combine it all to my SoccerBot and hopefully finish the project

Thanks again,
I own you at least a beer or a keg of beer