Path Planning after Monte Carlo Localization

This is where you talk about the NXJ software itself, installation issues, and programming talk.

Moderators: imaqine, 99jonathan, roger

Sally Ang
New User
Posts: 2
Joined: Thu Jan 19, 2017 10:17 am

Path Planning after Monte Carlo Localization

Postby Sally Ang » Thu Jan 19, 2017 10:57 am

Hi, everyone. Hopefully there is someone who can help me. I had tested with the MCLTest.java (NXT) and MCLCommand.java (PC) to test the Monte Carlo localization. I had also tested with the MapTest.java (NXT) and and PathFinding,java (PC) to test the calculatePath() and followPath(). Both works for me. However, I wish to perform something which is making the robot to be able to follow path from its current pose to the target (after the robot is being localized), I do not know how to make use of them. It keeps on throwing exception to me.

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

Re: Path Planning after Monte Carlo Localization

Postby gloomyandy » Thu Jan 19, 2017 6:39 pm

Hi firstly please try to post questions to the appropriate section of the forum. I have moved your topic to what is probably a better location.
Secondly you are asking for help with what is a pretty advanced question, to make it more likely for you to get a response then I think you need to provide more details of exactly what it is you wish to do. In particular...
a) What device is this code running on NXT or EV3?
b) It sounds like you have been running some of the sample code, can you provide more details of your experience with leJOS and Java, it will help ensure that any answers you get are "pitched at the right level".
c) You mention that you are getting exceptions thrown. We need to know more about what it is you have done to get into this state, and the details as to what exactly the exception is.
d) Is this some kind of college/school assignment or project?

To be honest you are asking for a lot of help with a pretty complex project. I'm not sure that anyone is going to be able to provide that help without understanding much more about your project, and I doubt if many people will be able to invest the time required to become that familiar with things to help. But certainly at the very least you need to provide much more information about exactly what you have done so far and what it is you are trying to do.

Good luck!
leJOS news https://lejosnews.wordpress.com/

Sally Ang
New User
Posts: 2
Joined: Thu Jan 19, 2017 10:17 am

Re: Path Planning after Monte Carlo Localization

Postby Sally Ang » Fri Jan 20, 2017 9:03 am

Hi, andy. Im working on the NXT model and using Eclipse java software. This is my uni project.I had stucked for a very long time. Below are my codes:

On the PC,

Code: Select all

package org.lejos.pcsample.mclcommand;
import java.awt.BorderLayout;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Point;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import javax.swing.JButton;
import javax.swing.JPanel;
import javax.swing.JPopupMenu;
import lejos.robotics.localization.MCLPoseProvider;
import lejos.robotics.mapping.MenuAction;
import lejos.robotics.mapping.NavigationModel.NavEvent;
import lejos.robotics.mapping.NavigationPanel;
import lejos.robotics.mapping.NavigationModel;
/**
 * Test of the Monte Carlo Localization algorithm for global localization.
 *
 * You should run MCLTest from the samples project on the NXT. See the comments in that sample for how
 * to set your robot up.
 *
 * You will need to set up a line map of your room (or other environment). This should be in the SVG file, Roon.svg.
 *
 * You can use a tool like svg-edit to set up this map. Make sure you only use <line> tags.
 *
 * To control your localization robot from the PC, run this sample and connect to your robot, by typing in
 * the name of the brick and pressing Connect.
 *
 * Then press Load Map, and your map will be loaded and displayed with a randomly generated particle set.
 *
 * You then control the robot by pressing Get Pose and Random Move. Get Pose will cause the robot to take
 * range readings and update its pose estimate. This will update the particle set.
 *
 * Random Move will cause the robot to make a random travel, followed by rotate move. Again the particle set will
 * be updated.
 *
 * Keep clicking Get Pose and Random Move until the robot has a good estimate of its pose. You should see the
 * particle set cluster around a few possible poses, and eventually find the correct pose.
 *
 * @author Lawrie Griffiths
 *
 */
public class MCLCommand extends NavigationPanel {
   private static final long serialVersionUID = 1L;

   private static final int FRAME_WIDTH = 1050;   //1050
   private static final int FRAME_HEIGHT = 700;   //700
   private static final int NUM_PARTICLES = 500;
   private static final String TITLE = "MCL Command";
   private static final int INITIAL_ZOOM = 200; //150
   private static final Point INITIAL_MAP_ORIGIN = new Point(7,0); //(-150,-30)
   private static final int MCL_CLEARANCE = 0;
   private static final Dimension MAP_AREA_SIZE = new Dimension(100,155);  //(800,500)
   //private static final NXTCommand nxtCommand = NXTCommandConnector.getSingletonOpen();
   
     private static int MESH_SPACE = 39;
     private static int CLEARANCE = 10;
   
   private final JPanel leftPanel = new JPanel();
   private final JPanel rightPanel = new JPanel();
   private final JButton randomButton = new JButton("Random move");
   private final JButton getPoseButton = new JButton("Get Pose");
   private final JButton rotateButton = new JButton("rotate");
   private final JButton calculateButton = new JButton("CalculatePath");
   private final JButton followButton = new JButton("FollowPath");
   
   private static MCLPoseProvider mcl = new MCLPoseProvider(null,NUM_PARTICLES,MCL_CLEARANCE);

     /**
      * Create a MapTest object and display it in a GUI frame.
      * Then connect to the NXT.
      */
     public static void main(String[] args) throws Exception {        
        (new MCLCommand()).run();
     }
 
     public MCLCommand() {
        setTitle(TITLE);
        setDescription("MCLCommand shows the Monte Carlo Localization\nalgorithm in action");
        
        buildGUI();
     }
 
     /**
      * Build the application-specific GUI
      */
     
    
     protected void buildGUI() {
        setLayout(new BorderLayout());
        // All panels required
       showZoomLabels = true;
       buildPanels();
      
       // Set the size of the map panel, and the viewport origin
       setMapPanelSize(MAP_AREA_SIZE);
       setMapOrigin(INITIAL_MAP_ORIGIN);
      
       // Add the Get Pose and Random Move buttons
      commandPanel.add(getPoseButton);
      commandPanel.add(randomButton);
      commandPanel.add(rotateButton);
      commandPanel.add(calculateButton);
      commandPanel.add(followButton);
      
      // disable buttons until connected
      getPoseButton.setEnabled(false);
      randomButton.setEnabled(false);
   
      showMesh = true;

      // When Get pose is pressed, invoke the MCL Pose provider
      // to take readings and get the pose. Then get the updated
      // particles, the details of the estimated pose and the range
      // readings.
      getPoseButton.addActionListener(new ActionListener() {
         public void actionPerformed(ActionEvent event) {
            model.getPose();            
            model.getRemoteParticles();
            model.getEstimatedPose();
            //System.out.println("Max weight:" + model.getParticles().getMaxWeight());
            model.getRemoteReadings();
            getPoseButton.setEnabled(false);         
         }
      });
      
      // When the Random Move button is pressed, make a random move
      // and get the updated particles
      randomButton.addActionListener(new ActionListener() {
         public void actionPerformed(ActionEvent event) {
            model.randomMove();
            model.getRemoteParticles();
         }
      });
      
      
      //when calculate button is clicked
      calculateButton.addActionListener(new ActionListener() {
         public void actionPerformed(ActionEvent event) {
            model.setPathFinder(0);
            model.calculatePath();            
            followButton.setEnabled(true);
         }
      });
      
      //when follow button is clicked
      followButton.addActionListener(new ActionListener() {
         public void actionPerformed(ActionEvent event) {
            //model.setPose(model.getRobotPose());
            //model.startNavigator();
            model.followPath();               
         }
      });
      
      
      
      //waypoint
      rotateButton.addActionListener(new ActionListener() {
         public void actionPerformed(ActionEvent event)  {                          
            model.rotate(90);      
      }
                  
      });
      
      // Switch on tool tips for particle weights
      mapPanel.setToolTipText("");
      
       // Add the required panels, configure them, and set their sizes
       rightPanel.setLayout(new BorderLayout());   
       loadPanel.setPreferredSize(new Dimension(300,70));
       leftPanel.add(loadPanel);
       connectPanel.setPreferredSize(new Dimension(300,90));
       leftPanel.add(connectPanel);      
       commandPanel.setPreferredSize(new Dimension(300,100));
       leftPanel.add(commandPanel);
       rightPanel.add(mapPanel, BorderLayout.CENTER);
       leftPanel.add(controlPanel);
       leftPanel.add(readingsPanel);
       readingsPanel.setPreferredSize(new Dimension(300,70));
       leftPanel.add(lastMovePanel);
       lastMovePanel.setPreferredSize(new Dimension(300,70));
       leftPanel.add(particlePanel);
       particlePanel.setPreferredSize(new Dimension(300,70));
       rightPanel.add(statusPanel, BorderLayout.SOUTH);
       leftPanel.setPreferredSize(new Dimension(320,650));
       add(leftPanel, BorderLayout.WEST);
       add(rightPanel, BorderLayout.CENTER);
       controlPanel.setPreferredSize(new Dimension(300,80));
       zoomSlider.setValue(INITIAL_ZOOM);
     }
 
     /**
      * Called when the mouse is clicked in the map area
      */
 

     

   protected void popupMenuItems(Point p, JPopupMenu menu) {
       // Get details of the particle closest to the mouse click
       menu.add(new MenuAction(NavEvent.FIND_CLOSEST, "Particle Readings", p, model, this));
       menu.add(new MenuAction(NavigationModel.NavEvent.SET_POSE, "Set pose", p , model, this));
       menu.add(new MenuAction(NavigationModel.NavEvent.SET_TARGET, "Set target", p, model, this));
   }
   
   /**
    * Called whenever an event is received from the NXT
    */

   public void eventReceived(NavEvent navEvent) {
      // Enable the Get Pose button when the estimated pose has been sent
      if (navEvent == NavEvent.ESTIMATED_POSE) {
         getPoseButton.setEnabled(true);
      } else if (navEvent == NavEvent.LOAD_MAP) {
         // Generate the particles
         model.generateParticles();      
      }
        else if (navEvent == NavEvent.PATH_COMPLETE) {
           // Add the path to the mesh again, when the path has been complete
           model.setPose(model.getRobotPose());
      }
   }
         
   /**
    * Called when the connection is established
    */
   
   public void whenConnected() {
      super.whenConnected();
      // If the map has been loaded, send it to the NXT
      if (model.getMap() != null) {
         model.sendMap();
         // Generate the particles
         model.generateParticles();
         //model.setPose(model.getRobotPose());
      }
      // Enable buttons
      getPoseButton.setEnabled(true);
      randomButton.setEnabled(true);
      //model.setPose(model.getRobotPose());
   }
   
   /**
    * Run the sample
    */
   public void run() throws Exception {
      // Set the NXT program to MCLTest
      program = "../MCLNXT_sample/MCLTest.nxj";
      //program = "../MCLNXT_sample/MapTest.nxj";
      
      // Set debugging on the model
      model.setDebug(true);   
      // Associate the MCLPoseProvider with the model
      model.setMCL(mcl);
      model.setMeshParams(MESH_SPACE, CLEARANCE);
      // Open the MCLTest navigation panel in a JFrame window
       openInJFrame(this, FRAME_WIDTH, FRAME_HEIGHT, TITLE, Color.WHITE, menuBar);
      
   }   
}


On the NXT:

Code: Select all

package org.lejos.sample.mcltest;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.FixedRangeScanner;
import lejos.robotics.RangeFinder;
import lejos.robotics.RangeScanner;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.RotatingRangeScanner;
import lejos.robotics.localization.MCLPoseProvider;
import lejos.robotics.mapping.NXTNavigationModel;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.navigation.Navigator;
import lejos.util.PilotProps;

/**
 * Used with MCLTest PC sample.
 * Change the parameters to suit your robot.
 * You can run the PilotParams sample to set up the parameters for DifferentialPilot.
 *
 * Run this sample on the NXT, then run MCLTest on the PC and connect to the NXT.
 *
 * This sample tests global localization. You can then put your robot down anywhere in a mapped room,
 * and watch the Monte Carlo Localization algorithm in action as it determines the location of the robot.
 *
 * The robot must use differential steering (unless you change the pilot used) and have an ultrasonic sensor
 * for taking range readings to walls and other features. a fixed or rotating range scanner can be used. In the fixed case,
 * the pilot rotates the robot to take range readings at different angles. In the rotating case, the ultrasonic
 * sensor must be mounted on a rotating platform.
 *
 * @author Lawrie Griffiths
 *
 */
public class MCLTest {
   private static final int GEAR_RATIO = -12;
   private static final boolean ROTATING_RANGE_SCANNER = false;
   private static final RegulatedMotor HEAD_MOTOR = Motor.A;
   private static final float[] ANGLES = {-45f,0f,45f};
   private static final int BORDER = 0;
   private static final double ROTATE_SPEED = 100f;
   private static final double TRAVEL_SPEED = 100f;
   private static final float MAX_DISTANCE = 60f;
   private static final float CLEARANCE = 20f;
   
   public static void main(String[] args) throws Exception {
       PilotProps pp = new PilotProps();
       pp.loadPersistentValues();
       float wheelDiameter = Float.parseFloat(pp.getProperty(PilotProps.KEY_WHEELDIAMETER, "3"));
       float trackWidth = Float.parseFloat(pp.getProperty(PilotProps.KEY_TRACKWIDTH, "12.8"));
       RegulatedMotor leftMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_LEFTMOTOR, "B"));
       RegulatedMotor rightMotor = PilotProps.getMotor(pp.getProperty(PilotProps.KEY_RIGHTMOTOR, "C"));
       boolean reverse = Boolean.parseBoolean(pp.getProperty(PilotProps.KEY_REVERSE,"false"));
       
       DifferentialPilot robot = new DifferentialPilot(wheelDiameter,trackWidth,leftMotor,rightMotor,reverse);
       robot.setRotateSpeed(ROTATE_SPEED);
       robot.setTravelSpeed(TRAVEL_SPEED);
       RangeFinder sonic = new UltrasonicSensor(SensorPort.S4);
       RangeScanner scanner;
       if (ROTATING_RANGE_SCANNER)scanner = new RotatingRangeScanner(HEAD_MOTOR, sonic, GEAR_RATIO);
       else scanner = new FixedRangeScanner(robot, sonic);
       scanner.setAngles(ANGLES);
       // Map and particles will be sent from the PC
       MCLPoseProvider mcl = new MCLPoseProvider(robot, scanner, null, 0, BORDER);
       Navigator navigator = new Navigator(robot, mcl);
   
       NXTNavigationModel model = new NXTNavigationModel();
       
   
       model.setDebug(true);
       model.setRandomMoveParameters(MAX_DISTANCE, CLEARANCE);
       // Adding the Navigator also adds the pilot, pose provider and scanner
       model.addNavigator(navigator);
       // Don't send the pose automatically - PC requests it when required
       model.setAutoSendPose(false);   
       navigator.followPath(model.getPath());
       navigator.waitForStop();
   }
}


There is no problem in the localization part. There is also no calculatePath() problem on the PC. However, on the PC side, when I click on the followButton to execute model.followPath(), the nxt throws Exceptions for me after making a rotation and a random move. I had no idea why. I think the path which consists of a list of waypoints had been sent to NXT and it just does not want to follow. Any missing stuff in my codes?

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

Re: Path Planning after Monte Carlo Localization

Postby gloomyandy » Fri Jan 20, 2017 9:33 am

Hi I think you should talk with your professor/teacher/supervisor and get them to help you with this. If you need to understand how to decode an exception on the NXT take a look at the following page:
http://www.lejos.org/nxt/nxj/tutorial/E ... ugging.htm
You need to understand what is causing the exception and from that work out what the problem is.
leJOS news https://lejosnews.wordpress.com/


Return to “NXJ Software”

Who is online

Users browsing this forum: No registered users and 3 guests