SimpleMulticopterMovement.java

/*
 * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
 *
 * This file is part of PeerfactSim.KOM.
 * 
 * PeerfactSim.KOM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 * 
 * PeerfactSim.KOM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

package de.tud.kom.p2psim.impl.topology.movement.aerial;


import java.util.LinkedHashMap;
import java.util.LinkedList;
import java.util.Map;
import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel;
import de.tud.kom.p2psim.impl.energy.components.ActuatorComponent;
import de.tud.kom.p2psim.impl.energy.components.StatefulActuatorComponent;
import de.tud.kom.p2psim.impl.topology.component.UAVTopologyComponent;
import de.tud.kom.p2psim.impl.topology.util.PositionVector;
import de.tudarmstadt.maki.simonstrator.api.Time;
import de.tudarmstadt.maki.simonstrator.api.uavsupport.callbacks.ReachedLocationCallback;

/**
 * Local movement for UAVs. 
 * This simple movement logic uses straight forward movement with the set speed (up to a max).
 * Energy consumption is based on state actuators.
 * 
 * @author Julian Zobel
 * @version 1.1, 14.01.2020
 */
public class SimpleMulticopterMovement implements UAVMovementModel  {

	private UAVTopologyComponent topologyComponent;	
	private double speed;	
	private double targetSpeed;
	
	private double maximumHorizontalSpeed = 15;
	private double maximumVerticalSpeed = 5;
	
	private LinkedList<PositionVector> route = new LinkedList<>();
	private Map<PositionVector, ReachedLocationCallback> locationCallbacks = new LinkedHashMap<>();  
		
	private StatefulActuatorComponent motor;
		
	public SimpleMulticopterMovement(UAVTopologyComponent topologyComponent) {
		this.topologyComponent = topologyComponent;			
	}	
	
	@Override
	public void move(long timeBetweenMovementOperations) {
				
		if(motor.isOn() && !route.isEmpty()) {		
		
			PositionVector currentPosition = topologyComponent.getRealPosition();
			
			PositionVector targetPosition = route.getFirst();
			Double distanceToTargetPosition = targetPosition.distanceTo(currentPosition);
							
			// If target point is reached within a 1 meter margin, we remove that point from the list 
			if(distanceToTargetPosition < 0.1 || distanceToTargetPosition < speed)
			{						
				route.removeFirst();
				
				topologyComponent.updateCurrentLocation(targetPosition); // triggers energy consumption for last distance
				speed = 0;
				motor.useActuator(0);
				locationReached(targetPosition);
				return;
			}
			
			double timefactor = timeBetweenMovementOperations / Time.SECOND;
			
			speed = Math.min(distanceToTargetPosition, targetSpeed);					
						
			PositionVector direction = new PositionVector(targetPosition);
			direction.subtract(currentPosition);
			direction.normalize();
			direction.multiplyScalar(speed * timefactor);

			PositionVector newPosition = new PositionVector(currentPosition);
			newPosition.add(direction);		
			
			motor.useActuator(1);
			topologyComponent.updateCurrentLocation(newPosition);					
		}
		else if(motor.isOn()) {
			
			if(speed != 0) {
				throw new UnsupportedOperationException("no route but speed not 0?");
			}
			
			PositionVector position = new PositionVector(topologyComponent.getRealPosition());	
			
			if(position.getAltitude() == 0) {
				motor.turnOff();
			}
			else {
				motor.useActuator(0);
			}
		}
	}
		
	@Override
	public void setPreferredSpeed(double v_pref) {
		this.targetSpeed = v_pref;		
	}
	
	@Override
	public double getCurrentSpeed() {
		return speed;
	}

	/**
	 * Trigger the callback function, if there is a valid callback 
	 * 
	 * @param position
	 */
	private void locationReached(PositionVector position) {
		if(locationCallbacks.containsKey(position)) {
			locationCallbacks.get(position).reachedLocation();
		}
	}
	
	@Override
	public void setTargetLocation(PositionVector target,
			ReachedLocationCallback reachedLocationCallback) {
		route.clear();
		route.add(target);
		if(reachedLocationCallback != null)
			locationCallbacks.put(target, reachedLocationCallback);
		
	}

	@Override
	public void setTargetLocationRoute(LinkedList<PositionVector> route,
			ReachedLocationCallback reachedLocationCallback) {
		this.route.clear();
		this.route.addAll(route);
		if(reachedLocationCallback != null)
			locationCallbacks.put(route.getLast(), reachedLocationCallback);
	}

	@Override
	public void addTargetLocation(PositionVector target,
			ReachedLocationCallback reachedLocationCallback) {
		route.add(target);
		if(reachedLocationCallback != null)
			locationCallbacks.put(target, reachedLocationCallback);
	}

	@Override
	public LinkedList<PositionVector> getTargetLocations() {
		
		LinkedList<PositionVector> copy = new LinkedList<>();
		for (PositionVector pv : route) {
			copy.add(pv.clone());		
		}
		
		return copy;
	}

	@Override
	public void removeTargetLocations() {
		route.clear();
		locationCallbacks.clear();
	}

	@Override
	public double estimatePowerConsumption(double velocity) {
		
		if(velocity == 0) {
			return motor.estimatePowerConsumption(0);
		}
		else { 
			return motor.estimatePowerConsumption(1.0);
		}
		
	}

	@Override
	public void setMotorControl(ActuatorComponent motor) {
		this.motor = (StatefulActuatorComponent) motor;
	}

	@Override
	public double verticalAscentMaxVelocity() {
		return maximumVerticalSpeed;
	}

	@Override
	public double horizontalMaxVelocity() {
		return maximumHorizontalSpeed;
	}
	
	@Override	
	public double horizontalMinVelocity() {
		return 0;
	}
}