Merge branch 'new_opentcs' of http://git.picaiba.com/agv/opentcs into new_opentcs

opentcs-api-base/src/main/java/org/opentcs/drivers/vehicle/VehicleProcessModel.java

@@ -91,6 +91,14 @@ public class VehicleProcessModel {
    * The vehicle's current bounding box.
    */
   private BoundingBox boundingBox;
+  /**
+   * 当前车辆级别
+   */
+  private Vehicle.IntegrationLevel integrationLevel;
+  /**
+   * 载货状态
+   */
+  private Boolean loadState;
 
   /**
    * Creates a new instance.
@@ -661,6 +669,7 @@ public class VehicleProcessModel {
       @Nonnull
       Vehicle.IntegrationLevel level
   ) {
+    integrationLevel = level;
     getPropertyChangeSupport().firePropertyChange(
         Attribute.INTEGRATION_LEVEL_CHANGE_REQUESTED.name(),
         null,
@@ -668,6 +677,14 @@ public class VehicleProcessModel {
     );
   }
 
+  /**
+   * 获取车辆级别
+   * @return 车辆级别
+   */
+  public Vehicle.IntegrationLevel getIntegrationLevel() {
+    return integrationLevel;
+  }
+
   /**
    * Notifies observers that the vehicle would like to have its current transport order withdrawn.
    *

opentcs-commadapter-loopback/src/main/java/org/opentcs/virtualvehicle/LoopbackCommunicationAdapter.java

@@ -492,28 +492,6 @@ public class LoopbackCommunicationAdapter
     getProcessModel().setState(Vehicle.State.EXECUTING);
     Step step = command.getStep();
 
-    Vehicle newVehicle = vehicleService.fetchObject(Vehicle.class, getProcessModel().getName());
-    List<Set<TCSResourceReference<?>>> allocatedResources = newVehicle.getAllocatedResources();
-
-    //使用副本更新车辆模型，防止异常情况
-    List<Set<TCSResourceReference<?>>> copiedResources = new ArrayList<>(allocatedResources);
-    copiedResources.clear();
-
-    if (step.getSourcePoint() != null) {
-      //下发起点不为空
-      Point point = objectService.fetchObject(Point.class, step.getSourcePoint().getName());
-      Set<TCSResourceReference<?>> resource = new HashSet<>();
-      resource.add(point.getReference());
-      copiedResources.add(resource);
-    }
-    if (step.getPath() != null) {
-      Path path = objectService.fetchObject(Path.class, step.getPath().getName());
-      Set<TCSResourceReference<?>> resource = new HashSet<>();
-      resource.add(path.getReference());
-      copiedResources.add(resource);
-    }
-    newVehicle.withAllocatedResources(copiedResources);
-
     if (step.getPath() == null) {
       actionExec(command);
     } else {
@@ -546,7 +524,7 @@ public class LoopbackCommunicationAdapter
       ACTION_STATUS = true;
 
       //下发动作
-      ExecuteAction.sendCmd(getProcessModel().getName(), command.getOperation(), getSerialNum());
+      ExecuteAction.sendCmd(command.getTransportOrder().getName(), getProcessModel().getName(), getProcessModel().getPosition(), command.getOperation(), getSerialNum());
 
       //进入阻塞
       while (ACTION_STATUS) {
@@ -893,13 +871,21 @@ public class LoopbackCommunicationAdapter
 
     //更新车辆姿态
     if (params.getPoint() != 0) {
-      LAST_PASSED_POINT = params.getPoint().toString();   //记录最终经过点
+//      LAST_PASSED_POINT = params.getPoint().toString();   //记录最终经过点
       if (!Objects.equals(getProcessModel().getPosition(), params.getPoint().toString())) {
-        getProcessModel().setPosition(params.getPoint().toString());    //更新最终经过点
+        initVehiclePosition(params.getPoint().toString());  //更新最终经过点
       }
 
+      Pose oldPose = getProcessModel().getPose();
       LAST_POSE = new Pose(new Triple(Math.round(params.getX() * 1000), Math.round(params.getY() * 1000), 0), params.getAngle());
-      getProcessModel().setPose(LAST_POSE);
+
+      if (oldPose.getPosition() == null) {    //姿态为空,更新
+        getProcessModel().setPose(LAST_POSE);
+      } else if (oldPose.getPosition().getX() != Math.round(params.getX() * 1000)
+          || oldPose.getPosition().getY() != Math.round(params.getY() * 1000)
+          || oldPose.getOrientationAngle() != params.getAngle()) {    //姿态有变化,更新
+        getProcessModel().setPose(LAST_POSE);
+      }
     } else {
       //最后经过点为0，应该是车辆丢失定位或重启过。todo 可能需要自动找点
     }
@@ -909,13 +895,19 @@ public class LoopbackCommunicationAdapter
     }
 
     //更新电量
-    getProcessModel().setEnergyLevel(Math.round(params.getPower() * 100));
+    if (getProcessModel().getEnergyLevel() != Math.round(params.getPower() * 100)) {
+      getProcessModel().setEnergyLevel(Math.round(params.getPower() * 100));
+    }
 
     //更新车辆等级
-    getProcessModel().integrationLevelChangeRequested(integrationLevel);
+    if (!Objects.equals(getProcessModel().getIntegrationLevel(), integrationLevel)) {
+      getProcessModel().integrationLevelChangeRequested(integrationLevel);
+    }
 
     //更新车辆状态
-    getProcessModel().setState(vehicleState);
+    if (!Objects.equals(getProcessModel().getState(), vehicleState)) {
+      getProcessModel().setState(vehicleState);
+    }
 
     //更新载货状态
     loadState = params.getCargo_status() == 1 ? LoadState.FULL : LoadState.EMPTY;
@@ -926,8 +918,13 @@ public class LoopbackCommunicationAdapter
     return serialNum;
   }
 
+  /**
+   * 处理车辆动作执行状态
+   * @param agvActionStatus 车辆动作执行状态对象
+   */
   private void handleActionStatus(AgvActionStatus agvActionStatus) {
     if (agvActionStatus.getStatus()) {
+//      System.out.println("handleActionStatus: ======");
       ACTION_STATUS = false;
     }
   }

opentcs-common/build.gradle

@@ -35,6 +35,8 @@ dependencies {
   implementation 'org.eclipse.paho:org.eclipse.paho.client.mqttv3:1.2.5'
   //JTS
   implementation 'org.locationtech.jts:jts-core:1.18.2'
+  // https://mvnrepository.com/artifact/org.locationtech.jts.io/jts-io-common
+//  implementation("org.locationtech.jts.io:jts-io-common:1.20.0")
 }
 
 processResources.doLast {

opentcs-common/src/main/java/org/opentcs/communication/http/dto/kc/action/RequestAction.java

@@ -8,9 +8,17 @@ import lombok.Data;
 @Data
 public class RequestAction {
 
+  /**
+   * 订单名称
+   */
+  private String order_name;
   /**
    * 动作
    */
   private String action;
+  /**
+   * 当前位置
+   */
+  private String point;
 
 }

opentcs-common/src/main/java/org/opentcs/communication/http/service/ExecuteAction.java

@@ -11,9 +11,13 @@ public class ExecuteAction extends BaseService {
 
   /**
    * 下发动作到平台
+   * @param orderName 车辆名称
+   * @param vehicleName 车辆名称
+   * @param point 当前位置
    * @param action 动作
+   * @param serialNum 序列号
    */
-  public static void sendCmd(String vehicleName, String action, Integer serialNum) {
+  public static void sendCmd(String orderName, String vehicleName, String point, String action, Integer serialNum) {
 
     String url = getUrl(vehicleName);
 
@@ -23,7 +27,9 @@ public class ExecuteAction extends BaseService {
     String time = now.format(formatter);
 
     RequestAction requestAction = new RequestAction();
+    requestAction.setOrder_name(orderName);
     requestAction.setAction(action);
+    requestAction.setPoint(point);
 
     BaseRequestTo baseRequestTo = new BaseRequestTo(
         4,

opentcs-common/src/main/java/org/opentcs/communication/http/service/ExecuteMove.java

@@ -177,7 +177,7 @@ public class ExecuteMove extends BaseService {
    */
   private static Integer getDrivePose(org.opentcs.data.model.Path path) {
     String drivePost = path.getProperties().get(LoopbackAdapterConstants.AGV_DRIVE_POSE);
-    System.out.println("getDrivePose drivePost: " + drivePost);
+//    System.out.println("getDrivePose drivePost: " + drivePost);
 
     int pose = 0;
     if (drivePost == null) {

opentcs-common/src/main/java/org/opentcs/manage/AdapterManage.java

@@ -84,18 +84,17 @@ public class AdapterManage {
       if (currentTime - value.getTime() > AUTO_CLOSE_TIME) {
         //当前时间减去记录时间大于阈值，自动关闭通讯适配器
         kernel.disableAdapter(key);
-        agvStatus.setStatus(AdapterStatus.DISABLE);
+//        agvStatus.setStatus(AdapterStatus.DISABLE);
+        adapterStatusMap.remove(key);
+        LOG.info("disable the adapter: {}", key);
       } else {
         //通讯适配器当前状态为关闭，设置状态为开启时才会进入
         kernel.enableAdapter(key);
         agvStatus.setStatus(AdapterStatus.ENABLE);
+        //更新记录数据
+        adapterStatusMap.put(key, agvStatus);
+        LOG.info("enable the adapter: {}", key);
       }
-
-      LOG.info("update the adapter: {} status: {}", key, value);
-
-      //更新记录数据
-      adapterStatusMap.put(key, agvStatus);
-//      LOG.info("adapterStatusMap end name: {}", key);
     });
   }
 

opentcs-common/src/main/java/org/opentcs/traffic/common/ContourAlgorithm.java

@@ -41,10 +41,6 @@ public class ContourAlgorithm {
     };
 
 
-//    new BezierCurve()
-
-
-
     return true;
   }
 

opentcs-kernel-extension-http-services/src/main/java/org/opentcs/kernel/extensions/servicewebapi/JsonBinder.java

@@ -7,6 +7,7 @@ import com.fasterxml.jackson.databind.ObjectMapper;
 import com.fasterxml.jackson.databind.SerializationFeature;
 import com.fasterxml.jackson.datatype.jsr310.JavaTimeModule;
 import java.io.IOException;
+import java.util.HashMap;
 
 /**
  * Binds JSON strings to objects and vice versa.
@@ -59,9 +60,14 @@ public class JsonBinder {
   public String toJson(Object object)
       throws IllegalStateException {
     try {
+      HashMap<String, Object> wrapper = new HashMap<>();
+      wrapper.put("data", object);
+      wrapper.put("code", 200);
+      wrapper.put("message", "success");
+
       return objectMapper
           .writerWithDefaultPrettyPrinter()
-          .writeValueAsString(object);
+          .writeValueAsString(wrapper);
     }
     catch (JsonProcessingException exc) {
       throw new IllegalStateException("Could not produce JSON output", exc);

opentcs-kernel-extension-http-services/src/main/java/org/opentcs/kernel/extensions/servicewebapi/v1/TransportOrderHandler.java

@@ -32,6 +32,7 @@ import org.opentcs.kernel.extensions.servicewebapi.KernelExecutorWrapper;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.GetOrderSequenceResponseTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.GetTransportOrderResponseTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostOrderSequenceRequestTO;
+import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostTransportOrderInfoRequestTo;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostTransportOrderRequestTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.posttransportorder.Destination;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.shared.Property;
@@ -59,6 +60,40 @@ public class TransportOrderHandler {
     this.executorWrapper = requireNonNull(executorWrapper, "executorWrapper");
   }
 
+  /**
+   * 根据WMS任务创建订单序列和运输订单
+   * @param orderName 订单名称
+//   * @param vehicleName 车辆名称
+   * @param type 订单类型
+   * @param destinations 位置信息
+   * @return 创建的订单
+   */
+  public TransportOrder createWmsTask(String orderName, String type, List<PostTransportOrderInfoRequestTo> destinations){
+
+    //订单目标点结构体
+    List<Destination> destinationsList = new ArrayList<>();
+    for (PostTransportOrderInfoRequestTo destination : destinations) {
+      Destination newDestination = new Destination();
+      newDestination.setLocationName(destination.getLocationName());
+      newDestination.setOperation(destination.getOperation());
+      destinationsList.add(newDestination);
+    }
+
+    //构建订单对象
+    PostTransportOrderRequestTO postTransportOrderRequestTO = new PostTransportOrderRequestTO();
+    postTransportOrderRequestTO.setIncompleteName(false);
+    postTransportOrderRequestTO.setDispensable(false);
+    postTransportOrderRequestTO.setDeadline(Instant.now());
+//    if (vehicleName != null) {
+//      postTransportOrderRequestTO.setIntendedVehicle(vehicleName);
+//    }
+    postTransportOrderRequestTO.setType(type);
+    postTransportOrderRequestTO.setDestinations(destinationsList);
+
+    //创建运输订单
+    return this.createOrder(orderName, postTransportOrderRequestTO);
+  }
+
   public TransportOrder createOrder(String name, PostTransportOrderRequestTO order)
       throws ObjectUnknownException,
         ObjectExistsException,

opentcs-kernel-extension-http-services/src/main/java/org/opentcs/kernel/extensions/servicewebapi/v1/V1RequestHandler.java

@@ -4,12 +4,14 @@ package org.opentcs.kernel.extensions.servicewebapi.v1;
 
 import static java.util.Objects.requireNonNull;
 
+import com.alibaba.fastjson.JSON;
 import jakarta.inject.Inject;
 import java.util.List;
 import java.util.concurrent.ExecutionException;
 import org.opentcs.access.KernelRuntimeException;
 import org.opentcs.data.ObjectExistsException;
 import org.opentcs.data.ObjectUnknownException;
+import org.opentcs.data.order.TransportOrder;
 import org.opentcs.kernel.extensions.servicewebapi.HttpConstants;
 import org.opentcs.kernel.extensions.servicewebapi.JsonBinder;
 import org.opentcs.kernel.extensions.servicewebapi.RequestHandler;
@@ -22,6 +24,7 @@ import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PlantModelTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostOrderSequenceRequestTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostPeripheralJobRequestTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostTopologyUpdateRequestTO;
+import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostTransportOrderInfoRequestTo;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostTransportOrderRequestTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostVehicleRoutesRequestTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostVehicleRoutesResponseTO;
@@ -293,6 +296,7 @@ public class V1RequestHandler
         request.body()
     );
     return "";
+//    return jsonBinder.toJson("");
   }
 
   private Object handlePostDispatcherTrigger(Request request, Response response)
@@ -370,14 +374,33 @@ public class V1RequestHandler
         IllegalArgumentException,
         IllegalStateException {
     response.type(HttpConstants.CONTENT_TYPE_APPLICATION_JSON_UTF8);
-    return jsonBinder.toJson(
+
-        GetTransportOrderResponseTO.fromTransportOrder(
+//    return jsonBinder.toJson(
-            transportOrderHandler.createOrder(
+//        GetTransportOrderResponseTO.fromTransportOrder(
-                request.params(":NAME"),
+//            transportOrderHandler.createOrder(
-                jsonBinder.fromJson(request.body(), PostTransportOrderRequestTO.class)
+//                request.params(":NAME"),
-            )
+//                jsonBinder.fromJson(request.body(), PostTransportOrderRequestTO.class)
-        )
+//            )
+//        )
+//    );
+
+    //解析data
+    String type = JSON.parseObject(request.body()).getString("type");
+    String destinations = JSON.parseObject(request.body()).getString("destinations");
+    List<PostTransportOrderInfoRequestTo> dataList = JSON.parseArray(destinations, PostTransportOrderInfoRequestTo.class);
+
+    //获取路由成本最低的车辆
+//    String executeVehicle = vehicleHandler.getExecuteVehicle(dataList.getFirst().getPoint());
+
+    //创建订单
+    TransportOrder wmsTask = transportOrderHandler.createWmsTask(
+        request.params(":NAME"),
+        type,
+        dataList
     );
+
+    //构建响应json
+    return jsonBinder.toJson(wmsTask);
   }
 
   private Object handlePutTransportOrderIntendedVehicle(Request request, Response response)
@@ -429,7 +452,7 @@ public class V1RequestHandler
         ExecutionException {
     transportOrderHandler.putOrderSequenceComplete(request.params(":NAME"));
     response.type(HttpConstants.CONTENT_TYPE_TEXT_PLAIN_UTF8);
-    return "";
+    return jsonBinder.toJson("");
   }
 
   private Object handlePostImmediateAssignment(Request request, Response response)

opentcs-kernel-extension-http-services/src/main/java/org/opentcs/kernel/extensions/servicewebapi/v1/VehicleHandler.java

@@ -28,6 +28,7 @@ import org.opentcs.data.model.Vehicle.EnergyLevelThresholdSet;
 import org.opentcs.data.order.Route;
 import org.opentcs.drivers.vehicle.VehicleCommAdapterDescription;
 import org.opentcs.drivers.vehicle.management.VehicleAttachmentInformation;
+import org.opentcs.drivers.vehicle.management.VehicleProcessModelTO;
 import org.opentcs.kernel.extensions.servicewebapi.KernelExecutorWrapper;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.GetVehicleResponseTO;
 import org.opentcs.kernel.extensions.servicewebapi.v1.binding.PostVehicleRoutesRequestTO;
@@ -68,7 +69,7 @@ public class VehicleHandler {
    * 接收平台异步回调处理
    */
   public void postReceiveCallback(Object data) {
-    System.out.println("jsonObject-----ssss: " + data.toString());
+//    System.out.println("jsonObject-----ssss: " + data.toString());
 
     //截取平台响应的字符串
     String jsonStr;
@@ -86,6 +87,22 @@ public class VehicleHandler {
       throw new ObjectUnknownException("postReceiveCallback Unknown vehicle: " + name);
     }
 
+    VehicleProcessModelTO vehicleProcessModelTO = vehicleService.fetchProcessModel(vehicle.getReference());
+    if (!vehicleProcessModelTO.isCommAdapterEnabled()) {
+      //未开启通讯适配器，手动选择通讯适配器开启
+      String adapterName = "org.opentcs.virtualvehicle.LoopbackCommunicationAdapterDescription";
+      VehicleCommAdapterDescription newAdapter
+          = vehicleService.fetchAttachmentInformation(vehicle.getReference())
+          .getAvailableCommAdapters()
+          .stream()
+          .filter(description -> description.getClass().getName().equals(adapterName))
+          .findAny()
+          .orElseThrow(
+              () -> new IllegalArgumentException("Unknown vehicle driver class name: " + adapterName)
+          );
+      vehicleService.attachCommAdapter(vehicle.getReference(), newAdapter);
+    }
+
     //将数据更新到线程安全的集合中，防止线程阻塞
     AdapterManage.setAdapterStatus(name);
     if (type == 1) {  //上报agv详细信息
@@ -99,6 +116,72 @@ public class VehicleHandler {
     }
   }
 
+  /**
+   * 获取执行成本最低车辆
+   * @param destinationPoint 目标点
+   * @return 车辆名称
+   */
+  public String getExecuteVehicle(String destinationPoint) {
+
+    //获取所有车辆
+    Set<Vehicle> vehicles = vehicleService.fetchObjects(Vehicle.class);
+    //设置终点
+    List<String> destinationPointList = List.of(destinationPoint);
+    //记录最低路由成本
+    long costs = 0;
+    //返回的车辆名称
+    String vehicleName = null;
+
+    for (Vehicle vehicle : vehicles) {
+      VehicleProcessModelTO vehicleProcessModelTO = vehicleService.fetchProcessModel(vehicle.getReference());
+      boolean commAdapterEnabled = vehicleProcessModelTO.isCommAdapterEnabled();
+
+      //校验车辆：通讯适配器状态、车辆状态、集成级别、订单
+      if (
+          !commAdapterEnabled ||
+          vehicle.getState() != Vehicle.State.IDLE ||
+          vehicle.getIntegrationLevel() != Vehicle.IntegrationLevel.TO_BE_UTILIZED ||
+          vehicle.getTransportOrder() != null
+      ) {
+        //车辆不能执行任务直接获取下台车辆
+        continue;
+      }
+
+      //获取车辆当前位置设置为起点
+      String sourcePoint = vehicle.getCurrentPosition().getName();
+
+      PostVehicleRoutesRequestTO postVehicleRoutesRequestTO = new PostVehicleRoutesRequestTO(destinationPointList);
+      postVehicleRoutesRequestTO.setSourcePoint(sourcePoint);
+      Map<TCSObjectReference<Point>, Route> vehicleRoutes = this.getVehicleRoutes(vehicle.getName(), postVehicleRoutesRequestTO);
+
+      for (Map.Entry<TCSObjectReference<Point>, Route> entry : vehicleRoutes.entrySet()) {
+//        TCSObjectReference<Point> key = entry.getKey();
+        Route value = entry.getValue();
+
+        //判断成本值是否合规(为空或小于0不合规)
+        if (value == null || value.getCosts() < 0) {
+          continue;
+        }
+
+        //比较成本，取成本低车辆名称
+        if (vehicleName == null || costs > value.getCosts()) {
+          //记录数据
+          costs = value.getCosts();
+          vehicleName = vehicle.getName();
+        }
+      }
+    }
+
+
+    if (vehicleName == null) {
+      //无空闲车辆需要创建订单无意向车辆订单
+//      throw new IllegalArgumentException("无可用车辆");
+      return "";
+    }
+
+    return vehicleName;
+  }
+
   /**
    * Find all vehicles orders and filters depending on the given parameters.
    *

opentcs-kernel-extension-http-services/src/main/java/org/opentcs/kernel/extensions/servicewebapi/v1/binding/PostTransportOrderInfoRequestTo.java

@@ -0,0 +1,43 @@
+package org.opentcs.kernel.extensions.servicewebapi.v1.binding;
+
+public class PostTransportOrderInfoRequestTo {
+
+  private String locationName;
+
+  private String point;
+
+  private String operation;
+
+  public String getLocationName() {
+    return locationName;
+  }
+
+  public void setLocationName(String locationName) {
+    this.locationName = locationName;
+  }
+
+  public String getPoint() {
+    return point;
+  }
+
+  public void setPoint(String point) {
+    this.point = point;
+  }
+
+  public String getOperation() {
+    return operation;
+  }
+
+  public void setOperation(String operation) {
+    this.operation = operation;
+  }
+
+  @Override
+  public String toString() {
+    return "PostTransportOrderInfoRequestTo{" +
+        "locationName='" + locationName + '\'' +
+        ", point='" + point + '\'' +
+        ", operation='" + operation + '\'' +
+        '}';
+  }
+}

opentcs-kernel/src/main/java/org/opentcs/kernel/vehicles/DefaultVehicleController.java

@@ -1032,6 +1032,14 @@ public class DefaultVehicleController
       Point currentVehiclePosition = originalCommand.getStep().getDestinationPoint();
       Deque<Set<TCSResource<?>>> allocatedResources
           = commandProcessingTracker.getAllocatedResources();
+
+//      KernelApplicationConfiguration.VehicleResourceManagementType vehicleResourceManagementType
+//          = configuration.vehicleResourceManagementType();
+//      System.out.println("vehicleResourceManagementType: " + vehicleResourceManagementType);
+//
+//      long length = commAdapter.getProcessModel().getBoundingBox().getLength();
+//      System.out.println("vehicle_length: " + length);
+
       switch (configuration.vehicleResourceManagementType()) {
         case LENGTH_IGNORED:
           while (!allocatedResources.peek().contains(currentVehiclePosition)) {
@@ -1044,11 +1052,13 @@ public class DefaultVehicleController
         case LENGTH_RESPECTED:
           // Free resources allocated for executed commands, but keep as many as needed for the
           // vehicle's current length.
+          long vehicleLeghtRadius = 3700 / 2;   //车辆长度半径
+          BoundingBox boundingBox = commAdapter.getProcessModel().getBoundingBox().withLength(vehicleLeghtRadius);  //长度有问题
           int freeableResourceSetCount
               = ResourceMath.freeableResourceSetCount(
                   SplitResources.from(allocatedResources, Set.of(currentVehiclePosition))
                       .getResourcesPassed(),
-                  commAdapter.getProcessModel().getBoundingBox().getLength()
+                  boundingBox.getLength()
               );
           for (int i = 0; i < freeableResourceSetCount; i++) {
             Set<TCSResource<?>> oldResources = allocatedResources.poll();

opentcs-kernel/src/main/java/org/opentcs/kernel/workingset/TransportOrderPoolManager.java

@@ -451,6 +451,7 @@ public class TransportOrderPoolManager
       throws ObjectUnknownException {
     TransportOrder order = getObjectRepo().getObject(TransportOrder.class, ref);
     // Make sure only orders in a final state are removed.
+    LOG.info("removeTransportOrder name: {}, state: {}", order.getName(), order.getState());
     checkArgument(
         order.getState().isFinalState(),
         "Transport order %s is not in a final state.",

opentcs-strategies-default/src/main/java/org/opentcs/strategies/basic/dispatching/phase/assignment/OrderAssigner.java

@@ -133,12 +133,12 @@ public class OrderAssigner {
     );
 
     AssignmentState assignmentState = new AssignmentState();
-    if (availableVehicles.size() < availableOrders.size()) {
+    if (availableVehicles.size() < availableOrders.size()) {  //车数量 < 订单数
       availableVehicles.stream()
           .sorted(vehicleComparator)
           .forEach(vehicle -> tryAssignOrder(vehicle, availableOrders, assignmentState));
     }
-    else {
+    else {    //车数量 >= 订单数
       availableOrders.stream()
           .sorted(orderComparator)
           .forEach(order -> tryAssignVehicle(order, availableVehicles, assignmentState));
@@ -227,13 +227,25 @@ public class OrderAssigner {
             )
             .collect(Collectors.partitioningBy(filterResult -> !filterResult.isFiltered()));
 
+//    // 调试输出：分组统计结果
+//    System.out.println("Candidate groups: Passed=" + ordersSplitByFilter.get(Boolean.TRUE).size() + ", Filtered=" + ordersSplitByFilter.get(Boolean.FALSE).size());
+//
+//    // 调试输出：显示所有通过过滤的候选（排序前）
+//    List<AssignmentCandidate> passedCandidates = ordersSplitByFilter.get(Boolean.TRUE).stream()
+//        .map(CandidateFilterResult::getCandidate)
+//        .collect(Collectors.toList());
+//
+//    passedCandidates.forEach(c -> System.out.println("passedCandidates - order: " + c.getTransportOrder() + ",vehicle:" + c.getVehicle() + ",Costs: " + c.getCompleteRoutingCosts()));
+
     ordersSplitByFilter.get(Boolean.FALSE).stream()
         .map(CandidateFilterResult::toFilterResult)
         .forEach(filterResult -> assignmentState.addFilteredOrder(filterResult));
 
+
     ordersSplitByFilter.get(Boolean.TRUE).stream()
         .map(CandidateFilterResult::getCandidate)
-        .sorted(vehicleCandidateComparator)
+//        .sorted(vehicleCandidateComparator)       框架原本排序有问题,修改为按照路由成本排序
+        .sorted(Comparator.comparingLong(AssignmentCandidate::getCompleteRoutingCosts))
         .findFirst()
         .ifPresent(candidate -> assignOrder(candidate, assignmentState));
   }