package messaging;

import org.ros.message.MessageListener;
import org.ros.message.rss_msgs.SonarMsg;

import LocalNavigation.LocalNavigationSolution;

public class SonarListener implements MessageListener<SonarMsg> {
	private LocalNavigationSolution localNav;
	private static double frontRange = -1;
	private static double rearRange = -1;

	public SonarListener(LocalNavigationSolution soln){
		localNav = soln;
	}

	@Override
	public void onNewMessage(SonarMsg msg) {
		synchronized(this){
			if (msg.isFront){
				frontRange = msg.range;
			} else {
				rearRange = msg.range;
			}
			if(frontRange < 0 || rearRange < 0){
				return;
			}
			localNav.handle(frontRange, rearRange);
			frontRange = -1;
			rearRange = -1;
		}
	}
}