SimulatedGPSNode.java

package com.roboclub.robobuggy.simulation;

import com.roboclub.robobuggy.messages.GpsMeasurement;
import com.roboclub.robobuggy.nodes.baseNodes.BuggyBaseNode;
import com.roboclub.robobuggy.nodes.baseNodes.PeriodicNode;
import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil;
import com.roboclub.robobuggy.ros.NodeChannel;
import com.roboclub.robobuggy.ros.Publisher;

import java.util.Date;

/**
 * A class for simulating the way that the gps system works to allow for offline testing
 *
 * @author Trevor Decker
 */
public class SimulatedGPSNode extends PeriodicNode {
    private Publisher gpsPub = new Publisher(NodeChannel.GPS.getMsgPath());
    private double noise = 0.5;

    /**
     * constructor for the simulated gps node
     *
     * @param period the number of milliseconds between gps update messages
     */
    public SimulatedGPSNode(int period) {
        super(new BuggyBaseNode(NodeChannel.GPS), period, "simulated_GPS");

        resume();
        // TODO Auto-generated constructor stub
    }

    @Override
    protected void update() {
        SimulatedBuggy simBuggy = SimulatedBuggy.getInstance();
        synchronized (this) {
            //TODO convert to lat lon

            double xVal = LocalizerUtil.convertMetersToLon(simBuggy.getX() + (2 * Math.random() - 1) * noise);
            double yVal = LocalizerUtil.convertMetersToLat(simBuggy.getY() + (2 * Math.random() - 1) * noise);
            gpsPub.publish(new GpsMeasurement(new Date(), new Date(), yVal, true, xVal, true, 0, 0, 0.0, 0.0));
        }
    }

    @Override
    protected boolean startDecoratorNode() {
        // TODO Auto-generated method stub
        return false;
    }

    @Override
    protected boolean shutdownDecoratorNode() {
        // TODO Auto-generated method stub
        return false;
    }


}