/* * 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 . * */ package de.tud.kom.p2psim.impl.topology.movement.aerial; import java.util.HashMap; import java.util.LinkedList; import java.util.Map; import de.tud.kom.p2psim.api.topology.movement.UAVMovementModel; 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 logic specifically designs the movement for multicopter UAVs. * This simple movement logic uses straight forward movement with the maximum speed available. * * @author Julian Zobel * @version 1.0, 11.09.2018 */ public class SimpleMutlicopterMovement implements UAVMovementModel { private UAVTopologyComponent topologyComponent; private final double maxCruiseSpeed; private final double minCruiseSpeed; private double preferredCruiseSpeed; private double currentSpeed; private LinkedList route = new LinkedList<>(); private Map locationCallbacks = new HashMap<>(); // TODO callback interface public SimpleMutlicopterMovement(UAVTopologyComponent topologyComponent, double maxCruiseSpeed, double minCruiseSpeed) { this.topologyComponent = topologyComponent; this.maxCruiseSpeed = maxCruiseSpeed; this.minCruiseSpeed = minCruiseSpeed; this.preferredCruiseSpeed = this.maxCruiseSpeed; } @Override public void setPreferredCruiseSpeed(double v_pref) { this.preferredCruiseSpeed = v_pref; } @Override public double getMaxCruiseSpeed() { return maxCruiseSpeed; } @Override public double getMinCruiseSpeed() { return minCruiseSpeed; } @Override public double getCurrentSpeed() { return currentSpeed; } @Override public void move(long timeBetweenMovementOperations) { if(topologyComponent.isActive() && !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 < currentSpeed) { route.removeFirst(); topologyComponent.updateCurrentLocation(targetPosition, 0.5); // triggers energy consumption for last distance currentSpeed = 0; topologyComponent.getActuatorEnergyComponent().useActuator(0); // now hover locationReached(targetPosition); return; } double timefactor = timeBetweenMovementOperations / Time.SECOND; currentSpeed = Math.min(distanceToTargetPosition, preferredCruiseSpeed); PositionVector direction = new PositionVector(targetPosition); direction.subtract(currentPosition); direction.normalize(); direction.multiplyScalar(currentSpeed * timefactor); PositionVector newPosition = new PositionVector(currentPosition); newPosition.add(direction); topologyComponent.updateCurrentLocation(newPosition, 1); } //System.out.println(Simulator.getFormattedTime(Simulator.getCurrentTime()) + " | " + topologyComponent); } /** * 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 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 getTargetLocations() { LinkedList copy = new LinkedList<>(); for (PositionVector pv : route) { copy.add(pv.clone()); } return copy; } @Override public void removeTargetLocations() { route.clear(); locationCallbacks.clear(); } }