GPSPoseProvider

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

Moderators: 99jonathan, roger, imaqine

GPSPoseProvider

Postby pepijndevos » Mon Mar 26, 2012 7:32 pm

Code: Select all
// Generated from GpsRobot.mirah
public class GPSPoseProvider extends java.lang.Object implements lejos.robotics.localization.PoseProvider, lejos.addon.gps.GPSListener {
  private lejos.addon.gps.GPS gps;
  private lejos.robotics.navigation.Pose pose;
  public  GPSPoseProvider(lejos.addon.gps.GPS gps) {
    this.gps = gps;
    this.pose = new lejos.robotics.navigation.Pose();
    lejos.addon.gps.GPS.addListener(((lejos.addon.gps.GPSListener)(this)));
  }
  public lejos.robotics.navigation.Pose getPose() {
    return this.pose;
  }
  public void setPose(lejos.robotics.navigation.Pose p) {
    this.pose = p;
  }
  public void sentenceReceived(lejos.addon.gps.NMEASentence sen) {
    float heading = 0;
    double lat = 0;
    double lon = 0;
    float y = 0;
    float x = 0;
    heading = this.gps.getCompassDegrees();
    lat = this.gps.getLatitude();
    lon = this.gps.getLongitude();
    y = ((float)((11131949 * lat)));
    x = ((float)(java.lang.Math.toRadians((6378137.0 * java.lang.Math.cos(java.lang.Math.toRadians(lat))))));
    GPSPoseProvider temp$1 = this;
    temp$1.setPose(new lejos.robotics.navigation.Pose(x, y, heading));
  }
}


Written in Mirah, does not work. Really.

Since I'm not sure I can express wheel size in lat/lon, I attempted the impossible, to convert lat/lon to centimeters.

I put that in a simple Navigator with a SteeringPilot, what it currently does is drive a forward arc, and then straight backwards, seemingly indefinitely.

I'll keep you up to date as I debug this, but I welcome any ideas.
pepijndevos
New User
 
Posts: 24
Joined: Fri May 14, 2010 8:56 am

Re: GPSPoseProvider

Postby gloomyandy » Mon Mar 26, 2012 8:38 pm

I think you need to take a step back and...
Explain what it is you are trying to do.
How your robot is constructed.
How you are trying to run this code (does it run on a pc or directly on the NXT).
Why you are using Mirah and how this fits with leJOS
What do you expect your robot to do?
What does it actually do?

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

Re: GPSPoseProvider

Postby pepijndevos » Tue Mar 27, 2012 5:54 pm

(today I had some fun attaching a RC receiver, beat that, IR link: http://www.youtube.com/watch?v=9Vnl4-xTfhA )
gloomyandy wrote:I think you need to take a step back and...
Explain what it is you are trying to do.

Make a robot that follows GPS waypoints.
gloomyandy wrote:How your robot is constructed.

It's basically a pimped version of the HiTechnic cart.
gloomyandy wrote:How you are trying to run this code (does it run on a pc or directly on the NXT).

Directly on the NXT, together with some other code, which I'll post below.
gloomyandy wrote:Why you are using Mirah and how this fits with leJOS

Because I like it. It just compiles to Java, but does away with the boilerplate.
gloomyandy wrote:What do you expect your robot to do?

Follow waypoints I wrote to a file using another program.
gloomyandy wrote:What does it actually do?

Well, it reads the waypoints, and then drives of in a random direction.

The screen is a little small for debugging.

This is the actual code:

Code: Select all
import lejos.robotics.localization.PoseProvider
import lejos.robotics.navigation.SteeringPilot
import lejos.robotics.navigation.Navigator
import lejos.nxt.Motor
import lejos.addon.gps.GPS
import lejos.addon.gps.GPSListener
import lejos.robotics.navigation.Pose
import java.lang.Math
import lejos.nxt.comm.Bluetooth
import lejos.nxt.comm.NXTConnection
import java.io.FileInputStream
import java.io.File
import java.io.DataInputStream
import java.io.IOException
import java.io.FileNotFoundException

class GPSPoseProvider
   implements PoseProvider
   implements GPSListener

   def initialize(gps:GPS)
      @gps = gps
      @pose = Pose.new()
      GPS.addListener(GPSListener(self))
   end

   def getPose
      @pose
   end

   def setPose(p)
      @pose = p
   end

   def sentenceReceived(sen)
      heading = @gps.getCompassDegrees
      lat = @gps.getLatitude
      lon = @gps.getLongitude
      y = float(11131949*lat)
      x = float(Math.toRadians(6378137.0 * Math.cos(Math.toRadians(lat))))
      self.setPose(Pose.new(x,y,heading))
   end
end

conn = Bluetooth.connect("Voda GPS", NXTConnection.RAW)
puts "connected!"

stream = conn.openInputStream()
location = GPS.new(stream)
location.setDaemon(false)

p = SteeringPilot.new(8.16, Motor.A, Motor.C, 100, 30, 30)
pp = GPSPoseProvider.new(location)

n = Navigator.new(p, pp)

begin
   log = DataInputStream.new(FileInputStream.new(File.new("path.log")))
   nil
rescue FileNotFoundException => ex
   puts "file not found"
   nil
end

while true
   begin
      x = log.readFloat()
      y = log.readFloat()
      puts "" + x + ", " + y
      n.addWaypoint(x, y)
      nil
   rescue IOException => ex
      puts "end"
      break
      nil
   end
end

n.followPath



and this the code for writing the coordinates:

Code: Select all
import lejos.nxt.comm.Bluetooth
import lejos.nxt.comm.NXTConnection
import lejos.addon.gps.GPS
import lejos.nxt.LCD
import lejos.addon.gps.GPSListener
import java.io.FileNotFoundException
import java.io.IOException
import java.io.File
import java.io.FileOutputStream
import java.io.DataOutputStream
import lejos.nxt.Button
import java.lang.Math

conn = Bluetooth.connect("Voda GPS", NXTConnection.RAW)

puts "connected!"

stream = conn.openInputStream()

location = GPS.new(stream)
location.setDaemon(false)

begin
   log = DataOutputStream.new(FileOutputStream.new(File.new("path.log")))
   nil
rescue FileNotFoundException => ex
   puts "file not found"
   nil
end


while true
   b = Button.waitForAnyPress
   if b == Button.ID_ENTER
      lat = location.getLatitude
      lon = location.getLongitude
      y = float(11131949*lat)
      x = float(Math.toRadians(6378137.0 * Math.cos(Math.toRadians(lat))))
      begin
         log.writeFloat(x)
         log.writeFloat(y)
         nil
      rescue IOException => ex
         puts "file error"
         nil
      end
   else
      begin
         log.close
         nil
      rescue IOException => ex
         puts "file error"
         nil
      end

      System.exit(0)
   end
end
pepijndevos
New User
 
Posts: 24
Joined: Fri May 14, 2010 8:56 am

Re: GPSPoseProvider

Postby gloomyandy » Tue Mar 27, 2012 7:38 pm

Hi,
Can't really help with any specifics but here are a few hints that may help...
Firstly are you sure that is a valid way to convert lat/long to x,y co-ordinates.
Secondly it is always good to look at your actual data so I would create a program that writes a text version of your x, y and heading values to a file. Then layout a course of markers on the ground and move your nxt to each point in turn following the path you would ideally have the robot take. Then upload the file to a PC so you can look at the values and perhaps plot them, Do they make sense? What happens if you wait a few minutes and then repeat the above, how closely do the two sets of data match? This is basically what you are expecting your robot to do. There is some degree of jitter in GPS values and a few metres is a large error when you are using a small robot like the nxt.
Thirdly you need to ensure that your co-ordinate system and your heading values make sense. I'm not sure which axis a heading of zero is aligned with but it may not be what you expect. You may want to step away from using the GPS for a while and simply layout some courses and follow them using the odometry based pose providers to make sure that you understand how the various parameters in the pose relate to each other...

Good luck...

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

Re: GPSPoseProvider

Postby pepijndevos » Wed Mar 28, 2012 11:19 am

gloomyandy wrote:Hi,
Can't really help with any specifics but here are a few hints that may help...
Firstly are you sure that is a valid way to convert lat/long to x,y co-ordinates.

There is no valid way to flatten a ellipsoid, but since the PoseProvider assumes a Cartesian space, I had to. So what this is is a crude way to get something Cartesian, hopefully good enough for navigating my back yard.

I verified that it approximates this table at Wikipedia: http://en.wikipedia.org/wiki/Longitude# ... _longitude
gloomyandy wrote:Secondly it is always good to look at your actual data so I would create a program that writes a text version of your x, y and heading values to a file. Then layout a course of markers on the ground and move your nxt to each point in turn following the path you would ideally have the robot take. Then upload the file to a PC so you can look at the values and perhaps plot them, Do they make sense? What happens if you wait a few minutes and then repeat the above, how closely do the two sets of data match? This is basically what you are expecting your robot to do. There is some degree of jitter in GPS values and a few metres is a large error when you are using a small robot like the nxt.
Thirdly you need to ensure that your co-ordinate system and your heading values make sense. I'm not sure which axis a heading of zero is aligned with but it may not be what you expect. You may want to step away from using the GPS for a while and simply layout some courses and follow them using the odometry based pose providers to make sure that you understand how the various parameters in the pose relate to each other...

I played with the OdometryPoseProvider for a bit, and it works great, unless the wheels slip.

Yea, I'll have to double-check the values I generate. I saw a few Bluetooth utilities, I'll try these.
gloomyandy wrote:
Good luck...

Andy
pepijndevos
New User
 
Posts: 24
Joined: Fri May 14, 2010 8:56 am

Re: GPSPoseProvider

Postby pepijndevos » Wed Mar 28, 2012 3:59 pm

Ok, I solved one problem, but I have a new one.

I figured the pilot initialization should have one negative tacho, which I assumed was the problem with turning.

Code: Select all
p = SteeringPilot.new(8.16/3, Motor.A, Motor.C, 50, -30, 30)


The formula I used was only calculating the length of a longitude, so I added the actual conversion.

Code: Select all
latcm = 11131949
loncm = Math.toRadians(637813700 * Math.cos(Math.toRadians(lon)))
x = float(latcm*lat)
y = float(loncm*lon)
self.setPose(Pose.new(x,y,heading))


As you can see, I swapped the x and y, because it was said that 0° was "parallel to the X axis". However, it does not work yet.

I can't find any reference which direction is the positive X/Y axis.

At least the robot kind of moves around in the correct area now. I'm going to try inverting the X axis.
pepijndevos
New User
 
Posts: 24
Joined: Fri May 14, 2010 8:56 am


Return to NXJ Software

Who is online

Users browsing this forum: Google [Bot], Yahoo [Bot] and 1 guest

more stuff