package com.roboclub.robobuggy.nodes.sensors;
import com.roboclub.robobuggy.main.RobobuggyLogicNotification;
import com.roboclub.robobuggy.main.RobobuggyMessageLevel;
import com.roboclub.robobuggy.messages.GpsMeasurement;
import com.roboclub.robobuggy.messages.StateMessage;
import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode;
import com.roboclub.robobuggy.nodes.baseNodes.NodeState;
import com.roboclub.robobuggy.nodes.baseNodes.SerialNode;
import com.roboclub.robobuggy.ros.NodeChannel;
import com.roboclub.robobuggy.ros.Publisher;
import java.io.UnsupportedEncodingException;
import java.text.DateFormat;
import java.text.ParseException;
import java.text.SimpleDateFormat;
import java.util.Date;
/**
* {@link SerialNode} for reading in GPS data
*
* @author Matt Sebek
* @author Kevin Brennan
*/
public final class GpsNode extends SerialNode {
private static final int BAUD_RATE = 9600;
// Used for state estimation
/*private static final double GYRO_PER = 0.9;
private static final double ACCEL_PER = 1 - GYRO_PER;
public double angle;
*/
private Publisher msgPub;
private Publisher statePub;
/**
* Creates a new {@link GpsNode}
*
* @param sensor {@link NodeChannel} of GPS
* @param portName name of the serial port to read from
*/
public GpsNode(NodeChannel sensor, String portName) {
super(new BuggyBaseNode(sensor), "GPS", portName, BAUD_RATE);
msgPub = new Publisher(sensor.getMsgPath());
statePub = new Publisher(sensor.getStatePath());
statePub.publish(new StateMessage(NodeState.DISCONNECTED));
}
/**
* {@inheritDoc}
*/
@Override
public boolean matchDataSample(byte[] sample) {
return true;
}
/**
* {@inheritDoc}
*/
@Override
public int matchDataMinSize() {
return 0;
}
/**
* {@inheritDoc}
*/
@Override
public int getBaudRate() {
return BAUD_RATE;
}
protected Date convertHHMMSStoTime(String timeHHMMSSss) {
//todo this may not be correct formatting
DateFormat f = new SimpleDateFormat("HHmmss");
try {
return f.parse(timeHHMMSSss);
} catch (ParseException e) {
new RobobuggyLogicNotification("Couldn't parse a time we got from peel!", RobobuggyMessageLevel.WARNING);
return new Date();
}
}
protected double convertMinutesSecondsToFloat(String posDDMMmmmmm) {
double posDD = Double.parseDouble(posDDMMmmmmm.substring(0, 2));
double posMM = Double.parseDouble(posDDMMmmmmm.substring(2));
return posDD + posMM / 60.0;
}
protected double convertMinSecToFloatLongitude(String dddmmmmm) {
double ddd = Double.parseDouble(dddmmmmm.substring(0, 3));
double mins = Double.parseDouble(dddmmmmm.substring(3));
return ddd + mins / 60.0;
}
/**
* {@inheritDoc}
*/
@Override
public int peel(byte[] buffer, int start, int bytesAvailable) {
String str;
try {
str = new String(buffer, start, bytesAvailable, "UTF-8");
} catch (UnsupportedEncodingException e) {
return 0;
}
// Quick check to reject things without doing string stuff (which is not cheap)
if (buffer[start] != '$') {
return 1;
}
if (buffer[start + 1] != 'G') {
return 1;
}
if (buffer[start + 2] != 'P') {
return 1;
}
if (buffer[start + 3] != 'G') {
return 1;
}
if (buffer[start + 4] != 'G') {
return 1;
}
if (buffer[start + 5] != 'A') {
return 1;
}
// Check the prefix.
String[] ar = str.split(",");
if (!ar[0].equals("$GPGGA")) {
new RobobuggyLogicNotification("saw a string other then $GPGGA that parsed properly:", RobobuggyMessageLevel.EXCEPTION);
setNodeState(NodeState.ERROR);
return 1; //that we dont publish this message
}
// Check for valid reading
int quality = Integer.parseInt(ar[6]);
if (quality == 0) {
// TODO publish not-lock to someone
setNodeState(NodeState.ERROR);
return 1;
}
Date readingTime = convertHHMMSStoTime(ar[1]);
double latitude = convertMinutesSecondsToFloat(ar[2]);
boolean north;
switch (ar[3]) {
case "N":
north = true;
break;
case "S":
north = false;
break;
default:
new RobobuggyLogicNotification("got a direction other then North or South in GPS node", RobobuggyMessageLevel.EXCEPTION);
setNodeState(NodeState.ERROR);
return 1; //that we dont publish this message
}
double longitude = convertMinSecToFloatLongitude(ar[4]);
boolean west;
switch (ar[5]) {
case "W":
west = true;
longitude = -1 * longitude;
break;
case "E":
west = false;
break;
default:
new RobobuggyLogicNotification("got a direction other then West or East in GPS node", RobobuggyMessageLevel.EXCEPTION);
setNodeState(NodeState.ERROR);
return 1; //that we dont publish this message
}
int numSatellites = Integer.parseInt(ar[7]);
double horizontalDilutionOfPrecision = Double.parseDouble(ar[8]);
double antennaAltitude = Double.parseDouble(ar[9]);
msgPub.publish(new GpsMeasurement(readingTime, latitude, north, longitude,
west, quality, numSatellites, horizontalDilutionOfPrecision, antennaAltitude,
Double.parseDouble(ar[2]), Double.parseDouble(ar[4])));
//Feed the watchdog
setNodeState(NodeState.ON);
return ar[0].length() + ar[1].length() + ar[2].length() + ar[3].length()
+ ar[4].length() + ar[5].length() + ar[6].length() + ar[7].length()
+ ar[8].length() + ar[9].length();
}
}