Here is a video from an unrelated MCL project. I am posting it so that you can see the basic idea.
Showing posts with label Monte Carlo Localization. Show all posts
Showing posts with label Monte Carlo Localization. Show all posts
Monday, January 12, 2009
Saturday, January 10, 2009
Test my New Program
Over the next few days, I'm going to be testing my new Monte Carlo Localization (MCL) application on different kinds of enclosures I want to see if the algorithm can handle different situations, localizing the robot correctly within the enclosure. Once I have some results, I will share them with you.
Sunday, January 4, 2009
The Communication Protocol
In communicating with the NXT robot, we want to keep our communication protocol as simple as possible. Through simplicity, the PC can direct more of its attention to the problem of Monte Carlo Localization (MCL), and less of its time on Bluetooth connection. The PC sends its command in the form of three integers. The first integer is the command itself, an integer from 0 to 4. The two integers that follow the command are optional parameters.
The robot’s reply is always eight integers. We need eight integers to accommodate the eight echoes that we obtain from the ultrasonic sensor.
There are five cases to consider:
The responsibility for sending the command to the robot and receiving a reply from it is shared between the Event Handler and BTCommPC classes. Upon detecting that an action event has been fired on the GUI, it is the Event Handler’s responsibility to gather the necessary information, compose the three-integer command and pass a reference to the integer array to the BTCommPC class. BTCommPC is responsible for sending the command one integer at a time to the robot and await a reply. Once a reply starts being sent, BTCommPC reads each integer, one at a time, placing them in an eight-integer array. BTCommPC then passes a reference to the array to the Event Handler.
Once the Event Handler receives a reference to the reply array, it is handler’s responsibility to disseminate it. Usually, this involves passing values back to the GUI panels and passing the ultrasonic sensor readings to the MCL class so that the MCL class can update its statistics. This command/reply process continues until the user requests that the Bluetooth connection be terminated either explicitly by pressing the disconnect button, or implicitly by terminating the program.
The robot’s program is complementary to BTCommPC’s algorithm. At first, the robot waits for a Bluetooth connection. Once a connection is made, it waits to receive its three-integer command. Once the command is received, the robot moves or turns, if necessary, and then sends back its eight-integer reply one integer at a time. The robot then waits for its next command. If the robot is commanded to terminate its Bluetooth connection, the robot sends back its acknowledgement, disconnects, and its program terminates on the brick.
More implementation to come.
The robot’s reply is always eight integers. We need eight integers to accommodate the eight echoes that we obtain from the ultrasonic sensor.
There are five cases to consider:
Description: The user requests the battery voltage
Command 0, 0, 0
Reply millivoltage, 0, 0, 0, 0, 0, 0, 0
Description: The user asks for the ultrasonic sensor to ping and report back
Command 1,0,0
Reply A series of 8 integers being the echo values
Description: The user requests that the robot move forward/backward
Command 2,distance,speed
Reply A series of 8 integers being the echo values
Description The user requests that the robot turn left/right
Command 3,angle,speed
Reply A series of 8 integers being the echo values
Description The user requests that the Bluetooth connection be terminated
Command 4,0,0
Reply 255,255,255,255,255,255,255,255
The responsibility for sending the command to the robot and receiving a reply from it is shared between the Event Handler and BTCommPC classes. Upon detecting that an action event has been fired on the GUI, it is the Event Handler’s responsibility to gather the necessary information, compose the three-integer command and pass a reference to the integer array to the BTCommPC class. BTCommPC is responsible for sending the command one integer at a time to the robot and await a reply. Once a reply starts being sent, BTCommPC reads each integer, one at a time, placing them in an eight-integer array. BTCommPC then passes a reference to the array to the Event Handler.
Once the Event Handler receives a reference to the reply array, it is handler’s responsibility to disseminate it. Usually, this involves passing values back to the GUI panels and passing the ultrasonic sensor readings to the MCL class so that the MCL class can update its statistics. This command/reply process continues until the user requests that the Bluetooth connection be terminated either explicitly by pressing the disconnect button, or implicitly by terminating the program.
The robot’s program is complementary to BTCommPC’s algorithm. At first, the robot waits for a Bluetooth connection. Once a connection is made, it waits to receive its three-integer command. Once the command is received, the robot moves or turns, if necessary, and then sends back its eight-integer reply one integer at a time. The robot then waits for its next command. If the robot is commanded to terminate its Bluetooth connection, the robot sends back its acknowledgement, disconnects, and its program terminates on the brick.
More implementation to come.
Putting it all Together: A working GUI
In this blog, I will explain DisplayPanel.java and then put everything together to make up the final GUI.
To understand and run the Display Panel, you must download Point.java, Line.java, and Pose.java. A point is a location in two-dimensional space; the class is the aggregate of two double precision numbers. In our implementation, a line is defined in terms of two points: a starting point and an ending point. A pose is a particular guess at the location and heading of the robot. It is defined in terms of heading, a point, and three other variables. Those three other variables are the pose’s distance, the shortest distance to the nearest boundary of the enclosure; its weight, the extent to which we believe that the pose is a correct estimate of the robot’s true heading and position; and finally its cumulative weight, the sum of the weights of all of the poses before it and itself.
The display panel looks like this
Admittedly, it’s not very impressive. No pose or boundary line information has been given to it to display. So, the panel simply shows its white background. Once pose and boundary information is given to the display panel, it will show the poses as red dots, and the boundaries and black lines.
Still, we’re now in a position to start putting everything together to get a GUI that looks like this:
The directory to the source code is here. To get the GUI running, execute it from Tester.java.
I have implemented the Connect button. Remember to substitute your own Bluetooth address for your own NXT in the Bluetooth address field of the GUI. Turn on your NXT. Click on the Connect button. You should be connected. The disconnect aspect of the button has not yet been implemented. However, you can achieve a disconnection by pressing on the enter and escape keys of your NXT to reset it. The Bluetooth connection will be lost.
The Localization class is the class responsible for assembling the GUI. It instantiates the panels and, using absolute positioning, places them on the content pane. Because we are using absolute positioning, we make the GUI non-resizable. If we left the GUI resizable, a resizing on the part of the user could cause panels to become hidden from the user.
The Localization class also instantiates the Event Handler, passing a reference to it to all of the panels. The implementation of the Bluetooth connection routine can be found in the Event Handler class.
As well, the Localization class instantiates the BTCommPC class. This class is responsible for opening the Bluetooth connection between the PC and the robot, closing the connection, and negotiating communication between the two. As soon as the Event Handler detects that a connect action event has been fired, the Event Handler calls BTCommPC’s open method to take the necessary steps to open the Bluetooth connection, if it can.
Because problems can arise in establishing a Bluetooth connection, exceptions can be thrown. When these exceptions are thrown, we want to put error messages up on the GUI, such as
To do this, we use the JOptionPane’s static showMessageDialog() method. However, that method requires that it know in which JFrame to draw the dialog box. Consequently, the Localization class passes a reference to itself when it instantiates the BTCommPC class.
Finally, the Localization class is responsible for listening to the user clicking on the close window button in the upper right-hand corner, because the close button is regarded as part of the JFrame and not as part of any of the JPanels. Before terminating the program, the Localization class puts put a confirmation window on the screen.
If the user clicks on the yes button, the Localization class attempts to programmatically click the disconnect button if it currently appears on the GUI. Only when the Bluetooth connection is closed does the program finally terminate.
If you should have any questions, please write to me at adcaine@gmail.com. See you next time when I will show you additional implementations.
To understand and run the Display Panel, you must download Point.java, Line.java, and Pose.java. A point is a location in two-dimensional space; the class is the aggregate of two double precision numbers. In our implementation, a line is defined in terms of two points: a starting point and an ending point. A pose is a particular guess at the location and heading of the robot. It is defined in terms of heading, a point, and three other variables. Those three other variables are the pose’s distance, the shortest distance to the nearest boundary of the enclosure; its weight, the extent to which we believe that the pose is a correct estimate of the robot’s true heading and position; and finally its cumulative weight, the sum of the weights of all of the poses before it and itself.
The display panel looks like this
Admittedly, it’s not very impressive. No pose or boundary line information has been given to it to display. So, the panel simply shows its white background. Once pose and boundary information is given to the display panel, it will show the poses as red dots, and the boundaries and black lines.
Still, we’re now in a position to start putting everything together to get a GUI that looks like this:
The directory to the source code is here. To get the GUI running, execute it from Tester.java.
I have implemented the Connect button. Remember to substitute your own Bluetooth address for your own NXT in the Bluetooth address field of the GUI. Turn on your NXT. Click on the Connect button. You should be connected. The disconnect aspect of the button has not yet been implemented. However, you can achieve a disconnection by pressing on the enter and escape keys of your NXT to reset it. The Bluetooth connection will be lost.
The Localization class is the class responsible for assembling the GUI. It instantiates the panels and, using absolute positioning, places them on the content pane. Because we are using absolute positioning, we make the GUI non-resizable. If we left the GUI resizable, a resizing on the part of the user could cause panels to become hidden from the user.
The Localization class also instantiates the Event Handler, passing a reference to it to all of the panels. The implementation of the Bluetooth connection routine can be found in the Event Handler class.
As well, the Localization class instantiates the BTCommPC class. This class is responsible for opening the Bluetooth connection between the PC and the robot, closing the connection, and negotiating communication between the two. As soon as the Event Handler detects that a connect action event has been fired, the Event Handler calls BTCommPC’s open method to take the necessary steps to open the Bluetooth connection, if it can.
Because problems can arise in establishing a Bluetooth connection, exceptions can be thrown. When these exceptions are thrown, we want to put error messages up on the GUI, such as
To do this, we use the JOptionPane’s static showMessageDialog() method. However, that method requires that it know in which JFrame to draw the dialog box. Consequently, the Localization class passes a reference to itself when it instantiates the BTCommPC class.
Finally, the Localization class is responsible for listening to the user clicking on the close window button in the upper right-hand corner, because the close button is regarded as part of the JFrame and not as part of any of the JPanels. Before terminating the program, the Localization class puts put a confirmation window on the screen.
If the user clicks on the yes button, the Localization class attempts to programmatically click the disconnect button if it currently appears on the GUI. Only when the Bluetooth connection is closed does the program finally terminate.
If you should have any questions, please write to me at adcaine@gmail.com. See you next time when I will show you additional implementations.
Connection, Command, and File Dump Panels
This blog entry is the continuation of my discussion of my project to demonstrate the idea behind Monte Carlo Localization (MCL) for robots using the NXT Mindstorms robot. The PC will run the MCL algorithm with the robot acting essentially like a mobile sensor platform. The PC and robot will communicate with each other over a Bluetooth connection. Here, I am going to discuss the connection panel, the command panel, and the file dump panel.
We want the Connection Panel to look like this
The single button at the top will act as both the connect and the disconnect button. The text on the button will alternate between the words connect, printed in green, and disconnect, printed in red. The status field, which cannot be edited by the user, will read either disconnected when the PC is not connected to the NXT robot, or connected when a Bluetooth connection has been established between the PC and the robot.
The only method that might need some explanation is this one:
This method is needed in case the user attempts to terminate the program before the robot is disconnected from the PC. If this should happen, we need a way to programmatically click the disconnect button before the program terminates. The doClick() method of the connectButton component does just exactly that.
I have primed the Bluetooth address field with my own NXT's Bluetooth address for the sake of personal convenience. You can change this by either commenting out or amending the following line of code.
You can download ConnectPanel.java here.
Another panel that we need is the Command panel. We want it to look like this
It has three buttons: initialize, move, and turn. The initialize button is used to re-start the MCL algorithm from scratch. The move button is used the move the robot a specified number of millimeters forward, if the distance is positive; backwards if the distance is negative. The turn button is used to turn the robot in place a specified number of degrees to the right, if the angle is negative; to the left if the angle is positive. The speed slider is used to adjust the robot’s speed.
We should emphasize that we are not trying to develop a remote control robot. Rather, the robot moves or turns in distinct stages. After each move or turn, students can observe how that move or turn has impacted the MCL algorithm. I am hoping that in this way students can learn how the MCL algorithm works through experimentation and feedback.
The following method may need some additional explanation:
A similar method also appears in the Statistics panel. This method is used to enable or disable the input components. The components are disabled whenever there is no connection between the PC and the robot; enabled whenever there is a connection between the PC and the robot. You can download CommandPanel.java here.
Another panel that we need is called the File Dump panel. We want the panel to look like this:
When the user clicks on the File Dump button, all of the statistics currently held by the MCL algorithm are written to a comma-separated value (csv) file using the index number on the spinner as part of the file’s file name. The spinner is automatically incremented to the next integer to avoid an accidental file overwrite. If the auto dump box is checked by the user, then the statistics are written to a csv file automatically after every move or turn of the robot. The spinner is automatically incremented to prevent an accidental file overwrite.
The purpose of the file dump panel is to allow the user to study the MCL algorithm's statistical data in another application such as Matlab or Excel to name two examples.
The File Dump button is always enabled, because there is always statistics to be written out to a file, even when the program is first initialized. You can download FileDump.java here.
The last panel that we will need is the Display Panel. I want to defer discussion of that panel until the next time I blog, because that panel touches very closely upon the MCL algorithm. While the class itself is not complicated as far as Java code goes, some of the things that the panel is doing might be unclear and require some explanation. If you should have any questions, please contact me at adcaine@gmail.com. See you next time.
We want the Connection Panel to look like this
The single button at the top will act as both the connect and the disconnect button. The text on the button will alternate between the words connect, printed in green, and disconnect, printed in red. The status field, which cannot be edited by the user, will read either disconnected when the PC is not connected to the NXT robot, or connected when a Bluetooth connection has been established between the PC and the robot.
The only method that might need some explanation is this one:
public void clickConnectButton(){
if(connectButton.getText().equals("Disconnect")){
connectButton.doClick();
}
}
This method is needed in case the user attempts to terminate the program before the robot is disconnected from the PC. If this should happen, we need a way to programmatically click the disconnect button before the program terminates. The doClick() method of the connectButton component does just exactly that.
I have primed the Bluetooth address field with my own NXT's Bluetooth address for the sake of personal convenience. You can change this by either commenting out or amending the following line of code.
addressField.setText("00:16:53:00:57:37");
You can download ConnectPanel.java here.
Another panel that we need is the Command panel. We want it to look like this
It has three buttons: initialize, move, and turn. The initialize button is used to re-start the MCL algorithm from scratch. The move button is used the move the robot a specified number of millimeters forward, if the distance is positive; backwards if the distance is negative. The turn button is used to turn the robot in place a specified number of degrees to the right, if the angle is negative; to the left if the angle is positive. The speed slider is used to adjust the robot’s speed.
We should emphasize that we are not trying to develop a remote control robot. Rather, the robot moves or turns in distinct stages. After each move or turn, students can observe how that move or turn has impacted the MCL algorithm. I am hoping that in this way students can learn how the MCL algorithm works through experimentation and feedback.
The following method may need some additional explanation:
public void setButtonsEnabled(boolean setting){
initialize.setEnabled(setting);
move.setEnabled(setting);
turn.setEnabled(setting);
speedSlider.setEnabled(setting);
}
A similar method also appears in the Statistics panel. This method is used to enable or disable the input components. The components are disabled whenever there is no connection between the PC and the robot; enabled whenever there is a connection between the PC and the robot. You can download CommandPanel.java here.
Another panel that we need is called the File Dump panel. We want the panel to look like this:
When the user clicks on the File Dump button, all of the statistics currently held by the MCL algorithm are written to a comma-separated value (csv) file using the index number on the spinner as part of the file’s file name. The spinner is automatically incremented to the next integer to avoid an accidental file overwrite. If the auto dump box is checked by the user, then the statistics are written to a csv file automatically after every move or turn of the robot. The spinner is automatically incremented to prevent an accidental file overwrite.
The purpose of the file dump panel is to allow the user to study the MCL algorithm's statistical data in another application such as Matlab or Excel to name two examples.
The File Dump button is always enabled, because there is always statistics to be written out to a file, even when the program is first initialized. You can download FileDump.java here.
The last panel that we will need is the Display Panel. I want to defer discussion of that panel until the next time I blog, because that panel touches very closely upon the MCL algorithm. While the class itself is not complicated as far as Java code goes, some of the things that the panel is doing might be unclear and require some explanation. If you should have any questions, please contact me at adcaine@gmail.com. See you next time.
Subscribe to:
Posts (Atom)
Posts
