org.opentcs.strategies.basic.dispatching.rerouting.RegularDriveOrderMerger Maven / Gradle / Ivy
/**
* Copyright (c) The openTCS Authors.
*
* This program is free software and subject to the MIT license. (For details,
* see the licensing information (LICENSE.txt) you should have received with
* this copy of the software.)
*/
package org.opentcs.strategies.basic.dispatching.rerouting;
import jakarta.inject.Inject;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.opentcs.components.kernel.Router;
import org.opentcs.data.model.Point;
import org.opentcs.data.order.ReroutingType;
import org.opentcs.data.order.Route;
import org.opentcs.strategies.basic.routing.ResourceAvoidanceExtractor;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* The {@link DriveOrderMerger} implementation for {@link ReroutingType#REGULAR}.
*
* Merges two drive orders so that the merged drive order follows the route of {@code orderA} up to
* the point where both drive orders ({@code orderA} and {@code orderB}) start to diverge. From
* there, the merged drive order follows the route of {@code orderB}.
*/
public class RegularDriveOrderMerger
extends
AbstractDriveOrderMerger {
private static final Logger LOG = LoggerFactory.getLogger(RegularDriveOrderMerger.class);
/**
* Creates a new instance.
*
* @param router The router to use.
* @param resourceAvoidanceExtractor Extracts resources to be avoided from transport orders.
*/
@Inject
public RegularDriveOrderMerger(
Router router,
ResourceAvoidanceExtractor resourceAvoidanceExtractor
) {
super(router, resourceAvoidanceExtractor);
}
@Override
protected List mergeSteps(
List stepsA,
List stepsB,
int currentRouteStepIndex
) {
LOG.debug("Merging steps {} with {}", stepsToPaths(stepsA), stepsToPaths(stepsB));
List mergedSteps = new ArrayList<>();
// Get the step where stepsB starts to diverge from stepsA (i.e. the step where routeA and
// routeB share the same source point).
Route.Step divergingStep = findStepWithSource(stepsB.get(0).getSourcePoint(), stepsA);
int divergingIndex = stepsA.indexOf(divergingStep);
mergedSteps.addAll(stepsA.subList(0, divergingIndex));
// Set the rerouting type for the first step in the new route.
Route.Step firstStepOfNewRoute = stepsB.get(0);
List modifiedStepsB = new ArrayList<>(stepsB);
modifiedStepsB.set(
0, new Route.Step(
firstStepOfNewRoute.getPath(),
firstStepOfNewRoute.getSourcePoint(),
firstStepOfNewRoute.getDestinationPoint(),
firstStepOfNewRoute.getVehicleOrientation(),
firstStepOfNewRoute.getRouteIndex(),
firstStepOfNewRoute.isExecutionAllowed(),
ReroutingType.REGULAR
)
);
mergedSteps.addAll(modifiedStepsB);
// Update the steps route indices since they originate from two different drive orders.
mergedSteps = updateRouteIndices(mergedSteps);
return mergedSteps;
}
private Route.Step findStepWithSource(Point sourcePoint, List steps) {
LOG.debug(
"Looking for a step with source point {} in {}",
sourcePoint,
stepsToPaths(steps)
);
return steps.stream()
.filter(step -> Objects.equals(step.getSourcePoint(), sourcePoint))
.findFirst()
.get();
}
}