How does the pathfinder work? (Destination unreachable error)

Post your NXJ projects, project ideas, etc here!

Moderators: imaqine, 99jonathan, roger

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Sun May 06, 2018 6:53 am

Hello,

In my project, I am trying to get my robot to navigate its way around an obstacle by drawing a map, and then navigating through that map using the Pathfinder class.

Since Pathfinder.findroute() uses an A* algorithm, I assumed that the required map needs to be a node graph. Thus, my idea of how the map should be generated works basically as the diagram below:

Image

As you can see here, once an object is detected as being in the way, the graph is altered to create alternate routes around the object.

However, as soon as I ran my project, the code would freeze at this specific line of code:

Code: Select all

tmpPath = pf.findRoute(orig, dest);


For about ~3 minutes before returning a "destination unreachable" exception. This is very puzzling, as you can see on the diagram, as the points for the start and destination are all definitely on the same map.

I debugged a bit, verified that all my geometry and map generation was 100% correct. Then I ran this test code:

Code: Select all

System.out.println(maze.inside(dest)); //The point of destination
System.out.println(maze.inside(loc.getLocation())); //The current pose of the robot
System.out.println(maze.inside(orig.getLocation())); //The robot's first position, thus the "origin" of the graph


This returned with the very puzzling response "False False False". So none of these points are on the map?

As a hunch, I ran an additional test code as a sanity check:

Code: Select all

System.out.println(maze.inside(maze.getLines()[0].getP1()));


So this code accesses the lines that make up the map (maze), gets the first point of the first line, and then checks if that point is in the very same map.

This code again gave the response "False".

Clearly, I must have some drastic lack of understanding as to how the Pathfinder class actually works, since its not doing anything remotely close to what I expect it to be doing. How does it work?

It is maddening that the API documentation has only a couple of sentences on each function. So many scenarios like this have no explanation.

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Sun May 06, 2018 8:36 am

First things first, it may be maddening to you, but please remember that all of this code is the result of people giving up their spare time to write it and that any help you get here is currently the result of basically me giving up my time to help you. Reading requests for help that contain negative comments does not make me particularly inclined to help out and is probably one of the reasons why the other leJOS contributors are no longer here. I know it can be frustrating at times, but all of the source is out there and it really is not hard to code to understand, timer spent reading it is time well spent.

Anyway back to your problem. Have you looked at the following sample....
https://sourceforge.net/p/lejos/nxt/cod ... nding.java
It should give you a good idea of how to construct a map (the code itself was originally written to run on a PC and use the leJOS PC API to remote control the robot.). There are other more advanced samples that should show you how to add obstacles to a map.The following may also be of use...
https://sourceforge.net/p/lejos/nxt/cod ... hTest.java
You may need to dig around a little in the source to find the supporting classes, but they are all there!
leJOS news https://lejosnews.wordpress.com/

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Mon May 07, 2018 1:19 am

Hello,

I apologize for any negative comments in my previous post. I do not mean to come off critical at all, I was just pointing out that if you ever want to update the documentation, it would be nice to include information about how a function like findRoute should be properly used.

Believe it or not, I used the example you provided as the base for my original code. Those examples came in my installation, and I come to the forum when I can't find the exact answers in the documentation or examples (of course some of it might be there and I overlooked).

I took the liberty of experimenting with the example code you provided (pathFinding) and chart out exactly how the robot behaves, so as to reverse engineer the way the robot should behave in my program. This is the path the robot takes when executing pathFinding:

Image

In this chart, blue indicates the lines used to create the map designed in the program, and red indicates the path the robot actually took when executed. I take note of the fact that the robot isn't following along the lines of the map at all, but rather takes effort to maneuver around the bounds of the map.

Ok, so I designed my program to just indicate a rectangular map around the bounds of an obstacle, and use the Pathfinder class to choose an arbitrary destination on the other side of the map. When experimenting with this implementation, I charted its map as follows:

Image

Once again, the blue lines indicate the bounds of my map (around a detected obstacle), and the other points on the left and right sides of the map are the robot's current position and destination (respectively).

At this point, I am still getting a destination unreachable exception. How is their map acceptable and mine is not? Aside from a rectangle verses a triangle, its the exact same thing. The only other way it could be explained is if the robot was somehow inside the map (I guess). But from my experiments I don't think that is happening.

Please, I am very sorry for coming off negative or critical before, I just really really want to figure this out.

Thanks

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Mon May 07, 2018 6:51 am

Hello again,

I think (emphasis on think) I might have solved that problem on my own, but I'm not sure as it still doesn't complete its command.

The findRoute function is implemented differently for each algorithm. For A*, it goes through every single node in the navigationMesh and evaluates which one is the optimum node for the next part of the path. If all nodes are exhausted and it still hasn't gotten to the goal, the program returns a Destination Unreachable exception.

However, if the navigationMesh is big enough (apparently more than 200 possible nodes), the A* algorithm seems to terminate early (possibly for taking too long). As it is terminated before finding the optimum path, it throws a Destination Unreachable exception, even if a possible path actually does exist.

At first I was able to eliminate the error by lowing the resolution of the navigationMesh (it was already pretty low because of a memory issue, but this time it was made lower), until it was able to complete the program.

However, A* is the only algorithm that even requires a navigation mesh. I switched it out for Dijkstra, and found that the entire debacle of navigationMesh could be avoided. It would have been incredibly easier if there as like, an example using Dijkstra or something.

Unfortunately, I can only tentatively say this solved the issue, because my robot still doesn't move. The robot works fine up until the point it calculates the path, and with test code I verified it successfully calculated a path, but at that point it completely freezes. No errors are being shown, no exceptions are being thrown, and any debug code I put in is never executed after this point. Even the sensors all seem to stop responding the instant the pathplanning is complete.

Fwiw, it seems like occasionally the program does completely work (yay), but so far that's only happened twice out of dozens of tries, with no discernible pattern between one run and another.

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Mon May 07, 2018 8:20 am

Note that the A* implementation is rather more sophisticated than that used for the other search mechanisms, in particular it has the notion of clearance around a wall or object to allow the robot to actually travel the found path. I can see no sign in the code that it will terminate early so if it is reporting no path found then either there is no path, or there is a bug in the code.

As to why the sample works but your code does not. Is your map the same size and resolution? Are you defining the same external boundary? Are you using the same grid spacing and clearance parameters? Why not define exactly the same map and odd a rectangular obstacle to it, rather than changing several things at once? Remember that you do not have to run this code on the NXT, you can easily run it on a PC and use the more powerful debug tools available there to work out what is going on.

You said your program is freezing, what exactly do you mean by freezing? How are you aborting it? If you can abort the program by using the ENTER+DOWN buttons on the NXT then the VM and your program is almost certainly still running, it may just be stuck looping in some part of the code. What was the last mehod called? Did it return? If you have sensors attached then it sounds like you are running more complex code than the sample. What happens if you just run the sample code? Start with a simple working program then add to it. You have all of the leJOS source code, add debug into the system classes to work out what is going on.

Perhaps you need to step back a little. What is this project you are working on? Why have you chosen to use an NXT and leJOS? Is it some sort of school/college project? The NXT and leJOS are now rather old. This means you will not get a lot of help with problems, you also need to evaluate if the hardware you are using is really capable of doing what you need it to do. The NXT is severely limited in memory compared to more modern devices (the EV3 has 64Mb compared to the NXT 64Kb). Also remember that although leJOS comes with things like A* etc. you do not have to use them. It may be better to implement your own from scratch (that way you will understand what it is doing far better), if you don't want to do that, be prepared to spend time understanding how the leJOS code works and any limitations it may have.
leJOS news https://lejosnews.wordpress.com/

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Mon May 07, 2018 5:49 pm

I did use the same resolution and clearance as the example. The bounding rectangle I had to define differently because the area my project takes place in is larger than the example. I got it to stop giving a destination unreachable exception once I lowered the resolution to the point that, given the different size of my map, it had the same total number of nodes.

In light of what you said, I went ahead and tried to run a helloworld program as a PC project, but I got the following error message:

Code: Select all

Native Library intelbth_x64 not available
Native Library bluecove_x64 not available
Failed to open connection to the NXT


Idk what this means. All of my NXT programs are connected via USB cable, and both the cable is plugged in and the robot is turned on. All the software I'm using is 32-bit, so idk why its asking about x64. So far, I have been able to debug enough as an NXT project, but I do admit that a debug tool would be helpful to figure out where in the code the robot is stuck at.

What I mean by freezing is that the robot stops moving after it planned the path. The last method called is the implemented featureDetected from the ultrasonic sensor, which is the same method I use to plan the path (when needed). A print statement right before the return statement is printed to the screen, so it must have returned, yet no other print statement seems to be hit after that point. The main method is wrapped in an infinite loop based on the light sensor. As soon as the light sensor is tripped at any time, the program terminates. However, once the program has reached this point, the light sensor no longer responds to being tripped, whereas it does respond if it is tripped before that point. I abort it using the ENTER+DOWN key, but I have no idea at what point the program is stuck at. If it's not in the featureDetected (because it just returned from it) and not in the main loop (because the light sensor is not responding) then where?

I did start with the example code and build off, with the exception that I copied code rather changing the original. I ran all the examples I used, and they all run exactly the same way every time as expected. I guess I don't consider my code that much of a leap from the examples, given that all the LeJOS packages works as expected. As I said before, if I had a way to debug into the code and find the exact point it is freezing, that would be very helpful.

It is a school project, and I am using NXT because it was assigned to us. I am using LeJOS because I'd rather not code a robot in C or Assembly, and there is no other languages available to me. I use a pathFinding search algorithm because 1) It's in the tools available to me 2) the example that uses it works 3) nothing in the documentation says I can't use it 4) I'd like to think it would be faster to figure out how to get an algorithm to work on my robot rather than making one from scratch. I understand there is a limitation of memory, and I will keep that in mind, but at the moment - for all I know - there may be just a simple issue that is getting it stuck at, rather than a limitation of resources.

I will go ahead and show you the code, so please, please if you don't mind, help me to get to the bottom of this issue.

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Mon May 07, 2018 5:55 pm

Code: Select all

public class NathanTest implements FeatureListener
{   
   //Construct robot
   public PilotProps pp = null;
   public RegulatedMotor leftMotor = null;
   public RegulatedMotor rightMotor = null;
   public DifferentialPilot robot = null;
   //Constants
   public float wheelDiameter = 0;
   public float trackWidth = 0;
   public boolean reverse = false;
   public boolean redraw = false;
   public boolean avoiding = false;
   //Sensors
   public UltrasonicSensor us = null;
   public RangeFeatureDetector fd = null;
   public LightSensor light = null;
   //Navigation
   public PoseProvider posep = null;
   public Navigator nav = null;
   //public AstarSearchAlgorithm alg = null;
   public Pose loc = null;
   Rectangle bounding = null;
   //Localization
   public LineMap maze = null;
   //public FourWayGridMesh grid = null;
   public PathFinder pf = null;
   public Waypoint dest = null;
   public List<obstacle> objects = null;
   
   //Custom class to represent an obstacle uniquely
   public class obstacle
   {
      private int x;
      private int y;
      //Constructor for double values caste to int
      public obstacle(double inx, double iny)
      {
         x = (int)inx;
         y = (int)iny;
      }
      //Overridden equals operator
      public boolean equals(Object other)
      {
         boolean retval = false;
         if (other instanceof obstacle)
         {
            obstacle obj = (obstacle) other;
            retval = (x == obj.x) && (y == obj.y);
         }
         return retval;
      }
      //Overridden hashcode
      public int hashCode()
      {
         return 1;
      }
   }
   
   public static void main(String[] args) throws IOException, InterruptedException
   {
      NathanTest project = new NathanTest();
      project.fd.addListener(project);
      project.objectAvoidance();
   }
   
   //Main constructor
   public NathanTest() throws IOException
   {
      pp = new PilotProps();
      pp.loadPersistentValues();
      wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "5.6"));
      trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "11.2"));
      leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "B"));
      leftMotor.setSpeed(100);
      rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "C"));
      rightMotor.setSpeed(100);
      reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE,"false"));
      robot = new DifferentialPilot(wheelDiameter,trackWidth,leftMotor,rightMotor,reverse);
      posep = new OdometryPoseProvider(robot);
      nav = new Navigator(robot, posep) ;
      us = new UltrasonicSensor(SensorPort.S1);
      us.setMode(1);
      fd = new RangeFeatureDetector(us, 80, 500);
      light = new LightSensor(SensorPort.S4);
      loc = posep.getPose();
      bounding = new Rectangle(-15,-155,620,310);
      maze = new LineMap();
      dest = new Waypoint(loc.getLocation());
      objects = new ArrayList<obstacle>();
   }
   
   //Main method
   public void objectAvoidance() throws InterruptedException
   {
      //Before reaching the first tape
      while(light.readValue() < 50)
      {
         robot.forward();
      }
      Thread.sleep(500);
      robot.stop();

      //After reaching the first tape
      while(light.readValue() < 50)
      {
         //Follow a path if we have it
         if (!nav.getPath().isEmpty())
         {
            //If we are working on a new path, stop
            if (redraw)
            {
               robot.stop();
            }
            else
            {
               nav.followPath();
            }
            
         }
         //Starting a new path
         if (nav.getPath().isEmpty() || nav.pathCompleted())
         {
            loc = posep.getPose();
            dest.setLocation((loc.getX()+100), loc.getY());
            nav.addWaypoint(dest);
         }
      }
      Thread.sleep(2000);
      robot.stop();
      return;
   }

   //This method is run if the ultrasonic sensor is activated
   public void featureDetected(Feature feature, FeatureDetector detector)
   {
      ArrayList<RangeReading> objectList = feature.getRangeReadings();
      loc = posep.getPose();
      //Loop through each object detected
      for (RangeReading object : objectList)
      {
         double margin = object.getRange()*Math.sin(object.getAngle());
         double theta = object.getAngle() + loc.getHeading();
         double rectX = (object.getRange()*Math.cos(theta))+loc.getX()-17;
         double rectY = (object.getRange()*Math.sin(theta))+loc.getY()-17;
         obstacle tmpObject = new obstacle(rectX, rectY);
         //If a new obstacle has been found on collision course
         //Adapt the map to it
         //26.5 long, 13.6 wide
         if (!objects.contains(tmpObject) && margin < 17 && object.getRange() < 60)
         {
            redraw = true;
            Rectangle tmpRect = new Rectangle((float)rectX, (float)rectY, (float)34, (float)34);
            //If the destination is inside the rectangle
            if(tmpRect.contains(dest))
            {
               dest.setLocation((tmpRect.getCenterX()+17), tmpRect.getCenterY());
            }
            maze = addRectToMap(maze, tmpRect);
            objects.add(tmpObject);
         }
      }
      //Optimize path along the map
      if (redraw)
      {
         pf = new DijkstraPathFinder(maze);
         Path tmpPath;
         loc = posep.getPose();
         //If we haven't set a path yet, set the destination
         if (nav.getPath().isEmpty())
         {
            dest.setLocation((loc.getX()+100), loc.getY());
         }
         //Check if the destination is unreachable (shouldn't happen)
         try
         {
            tmpPath = pf.findRoute(loc, dest);
            System.out.println(tmpPath.toString());
            nav.setPath(tmpPath);
         }
         catch (DestinationUnreachableException e)
         {
            System.out.println("Destination Unreachable");
            e.printStackTrace();
         }
         redraw = false;
      }
   }
   
   //Add a rectangle to the map
   public LineMap addRectToMap(LineMap map, Rectangle rect)
   {
      Line[] boundingRect = getBoundingLines(rect);
      Line[] tmpLineArry = map.getLines();
      //If the map is empty, add the bounding rectangle directly to it
      if (tmpLineArry != null)
      {
         Line[] newLineArry = new Line[tmpLineArry.length + 4];
         System.arraycopy(tmpLineArry, 0, newLineArry, 0, tmpLineArry.length);
         System.arraycopy(boundingRect, 0, newLineArry, tmpLineArry.length, 4);
         map = new LineMap(newLineArry, bounding);
      }
      else
      {
         map = new LineMap(boundingRect, bounding);
      }
      return map;
   }
   
   //Return an array of four lines that define the rectangle
   public Line[] getBoundingLines(Rectangle rect)
   {
      Line[] boundingRect = new Line[4];
      //Down
      boundingRect[0] = new Line((float)rect.getMinX(), (float)rect.getMinY(), (float)rect.getMinX(), (float)rect.getMaxY());
      //Left
      boundingRect[1] = new Line((float)rect.getMinX(), (float)rect.getMinY(), (float)rect.getMaxX(), (float)rect.getMinY());
      //Right
      boundingRect[2] = new Line((float)rect.getMinX(), (float)rect.getMaxY(), (float)rect.getMaxX(), (float)rect.getMaxY());
      //Up
      boundingRect[3] = new Line((float)rect.getMaxX(), (float)rect.getMinY(), (float)rect.getMaxX(), (float)rect.getMaxY());
      return boundingRect;
   }
}


I just didn't show the includes because I figure that is self explanatory

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Mon May 07, 2018 6:55 pm

That code is pretty complex. I wonder if you actually understand what may be happening?

The callback to your code for a detected feature will happen on a different thread to the one running your main code. This will be called over and over again so long as the feature is being detected (in your case you have set a delay of 500ms but it will take much longer than this for any object to be moved out of the view of the sensor). You have code that keeps track of detected objects, but looking at your test unless the detected item has exactly the same x and y co-ordinates then you will add a new object even if it overlaps a previously detected one. This is probably a bad idea as you will then be adding multiple objects into the path planning code that covers the same area.

You also have a number of variables that are shared between the two threads with no attempt to manage how they are accessed this is almost certainly a very bad idea. In particular the nav object can easily be changing after you have made tests on it so for instance you test for it being empty, but it could easily be set to empty by the other thread after you have tested it but before (or even worse during), attempting to follow the path.

I'm not sure what your experience is with Java programming but perhaps spending some time looking at some threading tutorials would be time well spent.

leJOS provides various tools to help you debug programs on the NXT, in particular you can arrange to send debug output to a console running on a PC. See:
http://www.lejos.org/nxt/nxj/tutorial/E ... ugging.htm
again time spent exploring these tools would not be wasted.

You should also probably spend some time understanding what results you actually get back from the feature detector and how to filter them so that you do not build up a long list of what is in effect the same object.

As I said earlier you can easily run some leJOS code on a PC (without needing an NXT at all), this would allow you to test the pathfinding classes for instance. You can't just run something like helloworld as this uses NXT specific code. To get a better idea for how to run PC based programs see the section on building PC programs here:
http://www.lejos.org/nxt/nxj/tutorial/P ... AndRun.htm
leJOS news https://lejosnews.wordpress.com/

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Tue May 08, 2018 12:35 am

Hello once again,

I made the code from scratch (what I didn't copy from examples), so I'm fairly confident I know how it works. Of course, if I knew what caused the issue I wouldn't be asking.

For your first suggestion, I went ahead and replaced the equals condition to be a maximum eculidean distance, rather than the same coordinates. I thought that it would be ok since I cast the coordinates to int (and therefore easier to make equal), but I realized that the ultrasonic sensor is not that accurate. For your second suggestion, I completely rewrote the main loop to eliminate as many shared variables between the two threads as possible, as well as declaring the shared boolean as volatile. It also reduced the complexity a bit, so it is easier then to debug.

I tried following your instructions on running the program on my PC, but I ran into the following issue:

Code: Select all

C:\Users\eclipse-workspace\NathanTest\src\nathanTest>nxjpcc NathanTest.java
C:\Users\eclipse-workspace\NathanTest\src\nathanTest>nxjpc NathanTest.class
Error: Could not find or load main class NathanTest.class
C:\Users\eclipse-workspace\NathanTest\src\nathanTest>nxjpc NathanTest
Error: Could not find or load main class NathanTest


However, after I made the changes you suggested, the performance has improved significantly, and it completes as expected about 80% of the time. I'll share my changed code so you can re-assess if there is any other glaring issues that could be affecting it.

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Tue May 08, 2018 12:39 am

Code: Select all

public class NathanTest implements FeatureListener
{   
   //Construct robot
   public PilotProps pp = null;
   public RegulatedMotor leftMotor = null;
   public RegulatedMotor rightMotor = null;
   public DifferentialPilot robot = null;
   //Constants
   public float wheelDiameter = 0;
   public float trackWidth = 0;
   public boolean reverse = false;
   public volatile boolean redraw = false;
   //Sensors
   public UltrasonicSensor us = null;
   public RangeFeatureDetector fd = null;
   public LightSensor light = null;
   //Navigation
   public PoseProvider posep = null;
   public Navigator nav = null;
   //public AstarSearchAlgorithm alg = null;
   public Pose loc = null;
   Rectangle bounding = null;
   //Localization
   public LineMap maze = null;
   public PathFinder pf = null;
   public Waypoint dest = null;
   public List<obstacle> objects = null;
   
   //Custom class to represent an obstacle uniquely
   public class obstacle
   {
      private double x;
      private double y;
      //Constructor for double values caste to int
      public obstacle(double inx, double iny)
      {
         x = inx;
         y = iny;
      }
      //Overloaded equals operator
      public boolean equals(obstacle other)
      {
         double xDiff = x-other.x; double yDiff = y-other.y;
         return Math.sqrt(xDiff*xDiff + yDiff*yDiff) < 50;
      }
   }
   
   public static void main(String[] args) throws IOException, InterruptedException
   {
      NathanTest project = new NathanTest();
      project.fd.addListener(project);
      project.objectAvoidance();
   }
   
   //Main constructor
   public FinalProjGoed() throws IOException
   {
      pp = new PilotProps();
      pp.loadPersistentValues();
      wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "5.6"));
      trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "11.2"));
      leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "B"));
      leftMotor.setSpeed(100);
      rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "C"));
      rightMotor.setSpeed(100);
      reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE,"false"));
      robot = new DifferentialPilot(wheelDiameter,trackWidth,leftMotor,rightMotor,reverse);
      posep = new OdometryPoseProvider(robot);
      nav = new Navigator(robot, posep) ;
      us = new UltrasonicSensor(SensorPort.S1);
      us.setMode(1);
      fd = new RangeFeatureDetector(us, 80, 500);
      light = new LightSensor(SensorPort.S4);
      loc = posep.getPose();
      bounding = new Rectangle(-15,-155,620,310);
      maze = new LineMap();
      dest = new Waypoint(loc.getLocation());
      objects = new ArrayList<obstacle>();
   }
   
   //Main method
   public void objectAvoidance() throws InterruptedException
   {
      //Before reaching the first tape
      while(light.readValue() < 50)
      {
         robot.forward();
      }
      Thread.sleep(500);
      robot.stop();

      //After reaching the first tape
      while(light.readValue() < 50)
      {
         //Keep the robot moving straight forward
         if (!redraw && !robot.isMoving() && nav.pathCompleted())
         {
            float angle = (-1)*posep.getPose().getHeading();
            robot.rotate(angle);
            robot.forward();
         }
      }
      Thread.sleep(2000);
      robot.stop();
      return;
   }

   //This method is run if the ultrasonic sensor is activated
   public void featureDetected(Feature feature, FeatureDetector detector)
   {
      ArrayList<RangeReading> objectList = feature.getRangeReadings();
      loc = posep.getPose();
      //Loop through each object detected
      for (RangeReading object : objectList)
      {
         double margin = object.getRange()*Math.sin(object.getAngle());
         double theta = object.getAngle() + loc.getHeading();
         double rectX = (object.getRange()*Math.cos(theta))+loc.getX()-17;
         double rectY = (object.getRange()*Math.sin(theta))+loc.getY()-17;
         obstacle tmpObject = new obstacle(rectX, rectY);
         boolean oldObject = false;
         //Check if we've seen the object already
         for (obstacle obj : objects)
         {
            oldObject = obj.equals(tmpObject);
         }
         //If a new obstacle has been found on collision course
         //Adapt the map to it
         //26.5 long, 13.6 wide
         if (!oldObject && object.getRange() < 50 && margin < 17)
         {
            Rectangle tmpRect = new Rectangle((float)rectX, (float)rectY, (float)34, (float)34);
            //If the destination is inside the rectangle
            if(tmpRect.contains(dest))
            {
               dest.setLocation((tmpRect.getCenterX()+17), tmpRect.getCenterY());
            }
            maze = addRectToMap(maze, tmpRect);
            objects.add(tmpObject);
            redraw = true;
         }
      }
      //Optimize path along the map
      if (redraw)
      {
         nav.clearPath();
         pf = new DijkstraPathFinder(maze);
         Path tmpPath;
         loc = posep.getPose();
         dest.setLocation((loc.getX()+100), loc.getY());
         //Check if the destination is unreachable (shouldn't happen)
         try
         {
            tmpPath = pf.findRoute(loc, dest);
            nav.followPath(tmpPath);
         }
         catch (DestinationUnreachableException e)
         {
            System.out.println("Destination Unreachable");
            e.printStackTrace();
         }
         redraw = false;
      }
   }
   
   //Add a rectangle to the map
   public LineMap addRectToMap(LineMap map, Rectangle rect)
   {
      Line[] boundingRect = getBoundingLines(rect);
      Line[] tmpLineArry = map.getLines();
      //If the map is empty, add the bounding rectangle directly to it
      if (tmpLineArry != null)
      {
         Line[] newLineArry = new Line[tmpLineArry.length + 4];
         System.arraycopy(tmpLineArry, 0, newLineArry, 0, tmpLineArry.length);
         System.arraycopy(boundingRect, 0, newLineArry, tmpLineArry.length, 4);
         map = new LineMap(newLineArry, bounding);
      }
      else
      {
         map = new LineMap(boundingRect, bounding);
      }
      return map;
   }
   
   //Return an array of four lines that define the rectangle
   public Line[] getBoundingLines(Rectangle rect)
   {
      Line[] boundingRect = new Line[4];
      //Down
      boundingRect[0] = new Line((float)rect.getMinX(), (float)rect.getMinY(), (float)rect.getMinX(), (float)rect.getMaxY());
      //Left
      boundingRect[1] = new Line((float)rect.getMinX(), (float)rect.getMinY(), (float)rect.getMaxX(), (float)rect.getMinY());
      //Right
      boundingRect[2] = new Line((float)rect.getMinX(), (float)rect.getMaxY(), (float)rect.getMaxX(), (float)rect.getMaxY());
      //Up
      boundingRect[3] = new Line((float)rect.getMaxX(), (float)rect.getMinY(), (float)rect.getMaxX(), (float)rect.getMaxY());
      return boundingRect;
   }
}

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Tue May 08, 2018 1:19 am

Sigh, if anything the new version has potentially more problems than the old. You are now accessing the Pilot from multiple threads, this is not a good thing to do. The leJOS classes (like most Java classes) are not thread safe and should not be used from multiple threads unless you take care to ensure that there is no chance of two methods being called at the same time. Consider what can happen during the robot.rotate call, there is nothing to stop the feature detection thread from trying to start following a path at the same time, this will eventually end up calling other methods on the pilot before the rotate has completed. This is not a good idea. You need to use critical sections in your threads to stop things like this happening. Even better don't access the pilot from both threads. A far better model would be to simply have the feature detection handler make a note of a new object if there is one and have the main thread handle everything else (including dealing with any changes required to the map due to new objects being detected).

Oh and in general it is almost always a bad idea to do very much processing (especially things that can block or take a long time) in code that is called from a thread that is not part of your own code. Most classes that use this sort of "notification model" expect that the method they call will return promptly and that the methods will not block. This is not the case with your feature detection handler.
leJOS news https://lejosnews.wordpress.com/

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Tue May 08, 2018 1:24 am

Oh and you should really be talking to your teacher/supervisor about this sort of thing. A leJOS forum is really not the place to be trying to teach someone about the problems of multi-threaded programming.
leJOS news https://lejosnews.wordpress.com/

Nathan1123
New User
Posts: 19
Joined: Fri Apr 27, 2018 10:05 pm

Re: How does the pathfinder work? (Destination unreachable error)

Postby Nathan1123 » Tue May 08, 2018 3:35 am

I'm very sorry if you feel like I'm wasting your time. In truth I came here more often because I considered my issues to be more a lack of knowledge about LeJOS, and much less an issue of coding practices in general. I did carefully consider the issue of the threads operating simultaneously, which is why I used the volatile boolean to switch off the functionality of one while the other is using the pilot.

At this point, the robot isn't freezing at all, it seems. Instead, it seems to have an issue with running straight into obstacles it sees. Sometimes it takes a path to avoid them, and sometimes it starts on its path and then veers directly into it. After running some test code, it definitely is not a case of the threads running at the same time. The section of code for moving forward is never called while the path is being used. The volatile boolean switches off that section first, before the nav.clearPath() causes the robot to stop, for instance. Rather, the behavior of the robot to run into the object seems to be the fault of the path constructed by the Dijkstra algorithm. I know the documentation says that the Dijkstra algorithm needs extra space in order to move around objects. However, I already gave a lot of space hard-coded in, and after I tried doubling the size of the rectangles the results didn't change.

I experimented with rewriting the implementation as you suggested, moving all the functionality into the main loop except for the update to the list of objects. However, the behavior I described above is exactly the same.

Again, I'm very sorry if you feel I am wasting your time, and if so I can try to just work it out myself from here.

User avatar
gloomyandy
leJOS Team Member
Posts: 6047
Joined: Fri Sep 28, 2007 2:06 pm
Location: UK

Re: How does the pathfinder work? (Destination unreachable error)

Postby gloomyandy » Tue May 08, 2018 7:57 am

I will make one final effort to point out some of your problems. Yes your boolean may stop the move forwards code being called while a path is being followed. However it does nothing to stop a new path being started while the robot is still executing the call to rotate. But this is just an indication that you may not have considered all of the possible threading issues. Code that uses threads is hard to understand and even harder to debug, just because your "test" shows it runs fine a few times does not mean it will run fine every time. That is why you need to have a very solid model for controlling access to shared objects (or even better don't share them).

Looking at your code I really don't think you have a grasp for how things may be changing while various parts of your code are executing. For instance your robot may well still be moving (thanks to your rotate and forward commands), while you are trying to process a detected object, you need to consider what impact that movement may have on things like the current pose and on your calculation of the location of the detected object not to mention how it may impact any paths you generate. By the time you have generated your new path the robot may no longer be where your code assumes it to be. Any errors like this will quickly accumulate if you do not take steps to deal with them.

My final comment is that if you have a problem that you think is being caused by code that is not your own as for instance your comment "seems to be the fault of the path constructed by the Dijkstra algorithm" would indicate then think very hard again. You are almost certainly wrong, it is very easy to try and blame other people's code, but more often than not it turns out to be a problem with your own code, or the assumptions you have made. You can easily waste a lot of time looking for problems in the wrong place and a good software engineer will never blame someone else's code until they have proven beyond any doubt that there really is an issue. The key is always to spend time understanding exactly what is actually happening and why rather than jumping to conclusions. I've worked with many engineers on lots of projects over 30+ years and have made these mistakes and seen others make them again and again.

But I'm sure you know best, good luck with your project.
leJOS news https://lejosnews.wordpress.com/


Return to “NXJ Projects”

Who is online

Users browsing this forum: No registered users and 1 guest