681. Next Closest Time

本文介绍了一种算法,该算法接收一个格式为“HH:MM”的时间字符串,并通过重复使用当前时间中的数字来构造出最接近且大于当前时间的时间。文章提供了两个实现方案,一种是基于TreeSet的数据结构进行逐位构建的方法,另一种是将时间转换为分钟数并逐分钟检查的解决方案。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

Given a time represented in the format "HH:MM", form the next closest time by reusing the current digits. There is no limit on how many times a digit can be reused.

You may assume the given input string is always valid. For example, "01:34", "12:09" are all valid. "1:34", "12:9" are all invalid.

Example 1:

Input: "19:34"
Output: "19:39"
Explanation: The next closest time choosing from digits 1, 9, 3, 4, is 19:39, which occurs 5 minutes later.  It is not 19:33, because this occurs 23 hours and 59 minutes later.

 

Example 2:

Input: "23:59"
Output: "22:22"
Explanation: The next closest time choosing from digits 2, 3, 5, 9, is 22:22. It may be assumed that the returned time is next day's time since it is smaller than the input time numerically.

根据旧时间,构造出大于旧时间的最小时间。

class Solution {
    private TreeSet<Integer> set = new TreeSet<>();
    
    public String nextClosestTime(String time) {
        char[] array = time.toCharArray();
        int a = array[0] - '0';
        int b = array[1] - '0';
        int c = array[3] - '0';
        int d = array[4] - '0';
        
        set.add(a);
        set.add(b);
        set.add(c);
        set.add(d);
        
        int n = getNext(d, 9);
        if (n > d) return new StringBuilder().append(a).append(b).append(":").append(c).append(n).toString();
        d = n;
        n = getNext(c, 5);
        if (n > c) return new StringBuilder().append(a).append(b).append(":").append(n).append(d).toString();
        c = n;
        
        if (a == 2) {
            n = getNext(b, 3);
        } else {
            n = getNext(b, 9);
        }
        if (n > b) return new StringBuilder().append(a).append(n).append(":").append(c).append(d).toString();
        b = n;
        n = getNext(a, 2);
        return new StringBuilder().append(n).append(b).append(":").append(c).append(d).toString();
    }
    
    private int getNext(int n, int max) {
        Integer next = set.higher(n);
        if (next != null && next <= max) return next;
        return set.iterator().next();
    }
}

input:"19:34"

基于treeset做的,从最小位,也就是分钟的个位,尝试寻找大于此值的最小值。大于4的最小值,也就是9。

如果没找到,那么就用4位数字中的最小值代替当前值。

继续向前尝试,也就是分钟的十位。

对于4位数字,只要有一个值找到了大于他的最小值,那么就结束。

举个栗子,input:"13:55"。

分钟的个位,大于5的最小值,没有,用1替代,十位也一样。=>13:11

小时的个位,找到大于3的最小值,5,替换一下。=>15:11

结束。

 

以下是solution:

class Solution {
    public String nextClosestTime(String time) {
        int cur = 60 * Integer.parseInt(time.substring(0, 2));
        cur += Integer.parseInt(time.substring(3));
        Set<Integer> allowed = new HashSet();
        for (char c: time.toCharArray()) if (c != ':') {
            allowed.add(c - '0');
        }

        while (true) {
            cur = (cur + 1) % (24 * 60);
            int[] digits = new int[]{cur / 60 / 10, cur / 60 % 10, cur % 60 / 10, cur % 60 % 10};
            search : {
                for (int d: digits) if (!allowed.contains(d)) break search;
                return String.format("%02d:%02d", cur / 60, cur % 60);
            }
        }
    }
}

想法也很特别,算出原始时间的分钟数,然后每次加一分钟,看一下是否能由旧时间构成。

java8写法,学习一下。

public class Passenger { int id; int arrivalTime; int startFloor; int targetFloor; int pickupTime = -1; int deliveryTime = -1; boolean pickedUp = false; int pickedUpBy = -1; public Passenger(int id, int arrivalTime, int startFloor, int targetFloor) { this.id = id; this.arrivalTime = arrivalTime; this.startFloor = startFloor; this.targetFloor = targetFloor; } public int getDirection() { return Integer.compare(targetFloor, startFloor); } public int getWaitingTime() { return pickupTime - arrivalTime; } public int getTotalTime() { return deliveryTime - arrivalTime; } @Override public String toString() { return "乘客" + id + " [" + startFloor + "→" + targetFloor + "]"; } } import java.util.*; import java.util.stream.Collectors; public class Elevator { public enum State { IDLE, MOVING, STOPPED } // 基本属性 int id; int currentFloor = 1; State state = State.IDLE; int passengers = 0; int capacity = 8; // 时间参数 int moveTime = 2; // 移动一层所需时间(秒) int stopTime = 3; // 停靠时间(秒) // 运行数据 List<Passenger> onboard = new ArrayList<>(); int timeCounter = 0; // 调度相关 int destinationFloor = -1; int direction = 0; Passenger currentTargetPassenger = null; Set<Integer> ignoredPassengerIds = new HashSet<>(); public Elevator(int id) { this.id = id; } /** * 设置目标乘客 */ public void setTargetPassenger(Passenger p, List<Elevator> allElevators) { if (p == null) return; // 清除旧目标的忽略标记 if (currentTargetPassenger != null) { allElevators.forEach(e -> e.ignoredPassengerIds.remove(currentTargetPassenger.id)); } this.currentTargetPassenger = p; destinationFloor = p.startFloor; direction = Integer.compare(destinationFloor, currentFloor); state = State.MOVING; // 通知其他电梯忽略此乘客 allElevators.stream() .filter(e -> e != this) .forEach(e -> e.ignoredPassengerIds.add(p.id)); } /** * 更新目标楼层 */ public void updateDestination() { // 优先级1: 当前目标乘客 if (currentTargetPassenger != null) { destinationFloor = currentTargetPassenger.startFloor; return; } // 优先级2: 电梯内乘客的目标楼层 if (!onboard.isEmpty()) { // 找到最近的乘客目标楼层 int closestFloor = onboard.get(0).targetFloor; int minDistance = Math.abs(closestFloor - currentFloor); for (Passenger p : onboard) { int distance = Math.abs(p.targetFloor - currentFloor); if (distance < minDistance) { minDistance = distance; closestFloor = p.targetFloor; } } destinationFloor = closestFloor; direction = Integer.compare(destinationFloor, currentFloor); return; } // 无目标 destinationFloor = -1; direction = 0; state = State.IDLE; } /** * 添加乘客到电梯 */ public void addPassenger(Passenger p, int currentTime, List<Elevator> allElevators) { if (passengers >= capacity) return; onboard.add(p); passengers++; p.pickupTime = currentTime; p.pickedUp = true; p.pickedUpBy = this.id; // 更新目的地 updateDestination(); // 从其他电梯的计划中移除该乘客 allElevators.stream() .filter(e -> e != this) .forEach(e -> { if (e.currentTargetPassenger == p) { e.currentTargetPassenger = null; e.updateDestination(); } }); } /** * 每秒更新电梯状态 */ public void updateState(int currentTime, List<Passenger> upWaiting, List<Passenger> downWaiting, List<Elevator> allElevators) { timeCounter++; switch (state) { case IDLE: // 空闲状态不执行操作 break; case MOVING: if (timeCounter >= moveTime) { currentFloor += direction; timeCounter = 0; // 检查是否到达目标楼层 if (currentFloor == destinationFloor) { state = State.STOPPED; } // 检查是否有顺路乘客 else if (checkForPickups(currentFloor, upWaiting, downWaiting)) { state = State.STOPPED; } } break; case STOPPED: if (timeCounter >= stopTime) { state = State.MOVING; timeCounter = 0; updateDestination(); } break; } } /** * 处理停靠点上下客 */ public void processStop(int currentTime, List<Passenger> upWaiting, List<Passenger> downWaiting, List<Elevator> allElevators) { // 1. 下客 List<Passenger> toUnload = new ArrayList<>(); for (Passenger p : onboard) { if (p.targetFloor == currentFloor) { toUnload.add(p); p.deliveryTime = currentTime; } } onboard.removeAll(toUnload); passengers -= toUnload.size(); // 2. 上客 - 目标乘客优先 if (currentTargetPassenger != null && currentTargetPassenger.startFloor == currentFloor && !currentTargetPassenger.pickedUp) { addPassenger(currentTargetPassenger, currentTime, allElevators); currentTargetPassenger = null; } // 3. 上客 - 顺路乘客 List<Passenger> directionPassengers = direction > 0 ? upWaiting : downWaiting; Iterator<Passenger> it = directionPassengers.iterator(); while (it.hasNext() && passengers < capacity) { Passenger p = it.next(); if (p.startFloor == currentFloor && !ignoredPassengerIds.contains(p.id) && !p.pickedUp) { addPassenger(p, currentTime, allElevators); it.remove(); } } // 更新目的地 updateDestination(); } // 检查当前楼层是否有可接载的乘客 private boolean checkForPickups(int floor, List<Passenger> upWaiting, List<Passenger> downWaiting) { List<Passenger> directionPassengers = direction > 0 ? upWaiting : downWaiting; for (Passenger p : directionPassengers) { if (p.startFloor == floor && !ignoredPassengerIds.contains(p.id) && !p.pickedUp) { return true; } } return false; } // 获取状态摘要 public String getStatus() { String stateStr = state == State.IDLE ? "空闲" : state == State.MOVING ? "移动中" : "停靠中"; String dirStr = direction > 0 ? "↑" : direction < 0 ? "↓" : ""; String targetStr = destinationFloor > 0 ? destinationFloor + "楼" : "无"; return String.format("电梯%d [%d楼] %s %s 乘客:%d 目标:%s", id, currentFloor, stateStr, dirStr, passengers, targetStr); } } import java.util.*; public class ElevatorSimulation { // 系统配置 static final int FLOORS = 8; static final int SIMULATION_TIME = 50; // 模拟50秒 static final int PASSENGER_RATE = 5; // 每5秒生成一个乘客 static final int PASSENGER_COUNT = 10; // 共生成10名乘客 public static void main(String[] args) { // 初始化两部电梯 List<Elevator> elevators = Arrays.asList( new Elevator(1), new Elevator(2) ); // 乘客队列 List<Passenger> upWaiting = new ArrayList<>(); List<Passenger> downWaiting = new ArrayList<>(); List<Passenger> allPassengers = new ArrayList<>(); // 初始化随机数生成器 Random rand = new Random(); int passengerId = 1; System.out.println("===== 电梯模拟系统 ====="); System.out.println("模拟时长: " + SIMULATION_TIME + "秒"); System.out.println("乘客生成: 每" + PASSENGER_RATE + "秒一个,共" + PASSENGER_COUNT + "名"); System.out.println("电梯数量: " + elevators.size() + "部"); System.out.println("=".repeat(40)); // 主模拟循环 for (int time = 0; time < SIMULATION_TIME; time++) { System.out.println("\n--- 时间: " + time + "秒 ---"); // 1. 每5秒生成一个新乘客(直到生成10名) if (time % PASSENGER_RATE == 0 && passengerId <= PASSENGER_COUNT) { int start = rand.nextInt(FLOORS) + 1; int target; do { target = rand.nextInt(FLOORS) + 1; } while (target == start); Passenger p = new Passenger(passengerId++, time, start, target); allPassengers.add(p); // 按方向添加到队列 if (p.getDirection() > 0) { upWaiting.add(p); System.out.println("新乘客: " + p + " ↑ (上行)"); } else { downWaiting.add(p); System.out.println("新乘客: " + p + " ↓ (下行)"); } } // 2. 更新电梯状态 for (Elevator elevator : elevators) { elevator.updateState(time, upWaiting, downWaiting, elevators); // 处理停靠状态 if (elevator.state == Elevator.State.STOPPED) { elevator.processStop(time, upWaiting, downWaiting, elevators); } // 显示电梯状态 System.out.println(elevator.getStatus()); } // 3. 解决电梯间冲突 resolveConflicts(elevators, time); // 4. 分配空闲电梯 assignIdleElevators(elevators, upWaiting, downWaiting, time); // 5. 显示等待队列 System.out.println("等待乘客: ↑" + upWaiting.size() + " ↓" + downWaiting.size()); } // 6. 输出最终统计 printStatistics(allPassengers); } /** * 解决电梯间目标冲突 */ static void resolveConflicts(List<Elevator> elevators, int currentTime) { // 检测目标乘客冲突 Map<Integer, List<Elevator>> targetMap = new HashMap<>(); for (Elevator e : elevators) { if (e.currentTargetPassenger != null) { int passengerId = e.currentTargetPassenger.id; targetMap.computeIfAbsent(passengerId, k -> new ArrayList<>()).add(e); } } // 解决冲突:每个乘客只分配给最近的电梯 for (Map.Entry<Integer, List<Elevator>> entry : targetMap.entrySet()) { if (entry.getValue().size() > 1) { int passengerId = entry.getKey(); List<Elevator> conflicting = entry.getValue(); // 找到最近的电梯 Elevator closest = null; int minDistance = Integer.MAX_VALUE; for (Elevator e : conflicting) { int distance = Math.abs(e.currentFloor - e.currentTargetPassenger.startFloor); if (distance < minDistance) { minDistance = distance; closest = e; } } // 其他电梯取消目标 for (Elevator e : conflicting) { if (e != closest) { e.currentTargetPassenger = null; e.updateDestination(); System.out.println("冲突解决: 电梯" + e.id + " 放弃乘客" + passengerId); } } } } } /** * 为所有空闲电梯分配任务 */ static void assignIdleElevators(List<Elevator> elevators, List<Passenger> upWaiting, List<Passenger> downWaiting, int currentTime) { // 获取所有空闲电梯 List<Elevator> idleElevators = new ArrayList<>(); for (Elevator e : elevators) { if (e.state == Elevator.State.IDLE) { idleElevators.add(e); } } if (idleElevators.isEmpty()) return; // 合并等待乘客 List<Passenger> allWaiting = new ArrayList<>(); allWaiting.addAll(upWaiting); allWaiting.addAll(downWaiting); // 过滤已被接走的乘客 allWaiting.removeIf(p -> p.pickedUp); if (allWaiting.isEmpty()) return; // 为每个空闲电梯分配最近的乘客 for (Elevator e : idleElevators) { // 查找最近的未分配乘客 Passenger closest = null; int minDistance = Integer.MAX_VALUE; for (Passenger p : allWaiting) { int distance = Math.abs(p.startFloor - e.currentFloor); if (distance < minDistance) { minDistance = distance; closest = p; } } if (closest != null) { e.setTargetPassenger(closest, elevators); allWaiting.remove(closest); System.out.println("分配: 电梯" + e.id + " → 乘客" + closest.id); } } } /** * 输出统计结果 */ static void printStatistics(List<Passenger> passengers) { System.out.println("\n===== 模拟结果 ====="); System.out.println("乘客总数: " + passengers.size()); int completed = 0; int totalWaitingTime = 0; int maxWaitingTime = 0; int totalTravelTime = 0; int maxTravelTime = 0; for (Passenger p : passengers) { if (p.deliveryTime != -1) { completed++; int waitingTime = p.getWaitingTime(); totalWaitingTime += waitingTime; if (waitingTime > maxWaitingTime) maxWaitingTime = waitingTime; int travelTime = p.getTotalTime(); totalTravelTime += travelTime; if (travelTime > maxTravelTime) maxTravelTime = travelTime; } } System.out.println("完成运输: " + completed + " 人 (" + (completed * 100 / passengers.size()) + "%)"); if (completed > 0) { System.out.println("平均等待时间: " + (totalWaitingTime / completed) + "秒"); System.out.println("最长等待时间: " + maxWaitingTime + "秒"); System.out.println("平均运输时间: " + (totalTravelTime / completed) + "秒"); System.out.println("最长运输时间: " + maxTravelTime + "秒"); } // 显示未完成乘客 List<Passenger> unfinished = passengers.stream() .filter(p -> p.deliveryTime == -1) .toList(); if (!unfinished.isEmpty()) { System.out.println("\n未完成乘客:"); for (Passenger p : unfinished) { String status = p.pickedUp ? "接载中" : "等待中"; System.out.println("乘客" + p.id + ": " + status + " (等待时间: " + (50 - p.arrivalTime) + "秒)"); } } } } 乘客生成的逻辑是什么 public class ElevatorSimulation { // 系统配置 static final int FLOORS = 8; static final int SIMULATION_TIME = 50; // 模拟50秒 static final int PASSENGER_RATE = 5; // 每5秒生成一个乘客 static final int PASSENGER_COUNT = 10; // 共生成10名乘客 public static void main(String[] args) { // 初始化两部电梯 List<Elevator> elevators = Arrays.asList( new Elevator(1), new Elevator(2) ); // 乘客队列 List<Passenger> upWaiting = new ArrayList<>(); List<Passenger> downWaiting = new ArrayList<>(); List<Passenger> allPassengers = new ArrayList<>(); // 初始化随机数生成器 Random rand = new Random(); int passengerId = 1; System.out.println("===== 电梯模拟系统 ====="); System.out.println("模拟时长: " + SIMULATION_TIME + "秒"); System.out.println("乘客生成: 每" + PASSENGER_RATE + "秒一个,共" + PASSENGER_COUNT + "名"); System.out.println("电梯数量: " + elevators.size() + "部"); System.out.println("=".repeat(40)); // 主模拟循环 for (int time = 0; time < SIMULATION_TIME; time++) { System.out.println("\n--- 时间: " + time + "秒 ---"); // 1. 每5秒生成一个新乘客(直到生成10名) if (time % PASSENGER_RATE == 0 && passengerId <= PASSENGER_COUNT) { int start = rand.nextInt(FLOORS) + 1; int target; do { target = rand.nextInt(FLOORS) + 1; } while (target == start); Passenger p = new Passenger(passengerId++, time, start, target); allPassengers.add(p); // 按方向添加到队列 if (p.getDirection() > 0) { upWaiting.add(p); System.out.println("新乘客: " + p + " ↑ (上行)"); } else { downWaiting.add(p); System.out.println("新乘客: " + p + " ↓ (下行)"); } } // 2. 更新电梯状态 for (Elevator elevator : elevators) { elevator.updateState(time, upWaiting, downWaiting, elevators); // 处理停靠状态 if (elevator.state == Elevator.State.STOPPED) { elevator.processStop(time, upWaiting, downWaiting, elevators); } // 显示电梯状态 System.out.println(elevator.getStatus()); } // 3. 解决电梯间冲突 resolveConflicts(elevators, time); // 4. 分配空闲电梯 assignIdleElevators(elevators, upWaiting, downWaiting, time); // 5. 显示等待队列 System.out.println("等待乘客: ↑" + upWaiting.size() + " ↓" + downWaiting.size()); } // 6. 输出最终统计 printStatistics(allPassengers); } /** * 解决电梯间目标冲突 */ static void resolveConflicts(List<Elevator> elevators, int currentTime) { // 检测目标乘客冲突 Map<Integer, List<Elevator>> targetMap = new HashMap<>(); for (Elevator e : elevators) { if (e.currentTargetPassenger != null) { int passengerId = e.currentTargetPassenger.id; targetMap.computeIfAbsent(passengerId, k -> new ArrayList<>()).add(e); } } // 解决冲突:每个乘客只分配给最近的电梯 for (Map.Entry<Integer, List<Elevator>> entry : targetMap.entrySet()) { if (entry.getValue().size() > 1) { int passengerId = entry.getKey(); List<Elevator> conflicting = entry.getValue(); // 找到最近的电梯 Elevator closest = null; int minDistance = Integer.MAX_VALUE; for (Elevator e : conflicting) { int distance = Math.abs(e.currentFloor - e.currentTargetPassenger.startFloor); if (distance < minDistance) { minDistance = distance; closest = e; } } // 其他电梯取消目标 for (Elevator e : conflicting) { if (e != closest) { e.currentTargetPassenger = null; e.updateDestination(); System.out.println("冲突解决: 电梯" + e.id + " 放弃乘客" + passengerId); } } } } } /** * 为所有空闲电梯分配任务 */ static void assignIdleElevators(List<Elevator> elevators, List<Passenger> upWaiting, List<Passenger> downWaiting, int currentTime) { // 获取所有空闲电梯 List<Elevator> idleElevators = new ArrayList<>(); for (Elevator e : elevators) { if (e.state == Elevator.State.IDLE) { idleElevators.add(e); } } if (idleElevators.isEmpty()) return; // 合并等待乘客 List<Passenger> allWaiting = new ArrayList<>(); allWaiting.addAll(upWaiting); allWaiting.addAll(downWaiting); // 过滤已被接走的乘客 allWaiting.removeIf(p -> p.pickedUp); if (allWaiting.isEmpty()) return; // 为每个空闲电梯分配最近的乘客 for (Elevator e : idleElevators) { // 查找最近的未分配乘客 Passenger closest = null; int minDistance = Integer.MAX_VALUE; for (Passenger p : allWaiting) { int distance = Math.abs(p.startFloor - e.currentFloor); if (distance < minDistance) { minDistance = distance; closest = p; } } if (closest != null) { e.setTargetPassenger(closest, elevators); allWaiting.remove(closest); System.out.println("分配: 电梯" + e.id + " → 乘客" + closest.id); } } } /** * 输出统计结果 */ static void printStatistics(List<Passenger> passengers) { System.out.println("\n===== 模拟结果 ====="); System.out.println("乘客总数: " + passengers.size()); int completed = 0; int totalWaitingTime = 0; int maxWaitingTime = 0; int totalTravelTime = 0; int maxTravelTime = 0; for (Passenger p : passengers) { if (p.deliveryTime != -1) { completed++; int waitingTime = p.getWaitingTime(); totalWaitingTime += waitingTime; if (waitingTime > maxWaitingTime) maxWaitingTime = waitingTime; int travelTime = p.getTotalTime(); totalTravelTime += travelTime; if (travelTime > maxTravelTime) maxTravelTime = travelTime; } } System.out.println("完成运输: " + completed + " 人 (" + (completed * 100 / passengers.size()) + "%)"); if (completed > 0) { System.out.println("平均等待时间: " + (totalWaitingTime / completed) + "秒"); System.out.println("最长等待时间: " + maxWaitingTime + "秒"); System.out.println("平均运输时间: " + (totalTravelTime / completed) + "秒"); System.out.println("最长运输时间: " + maxTravelTime + "秒"); } // 显示未完成乘客 List<Passenger> unfinished = passengers.stream() .filter(p -> p.deliveryTime == -1) .toList(); if (!unfinished.isEmpty()) { System.out.println("\n未完成乘客:"); for (Passenger p : unfinished) { String status = p.pickedUp ? "接载中" : "等待中"; System.out.println("乘客" + p.id + ": " + status + " (等待时间: " + (50 - p.arrivalTime) + "秒)"); } } } }
06-23
这是我object.py的内容# coding=utf-8 from .collision import * from .graphics import load_texture from .utils import get_file_path class WorldObj: def __init__(self, obj, domain_rand, safety_radius_mult): """ Initializes the object and its properties """ # XXX this is relied on by things but it is not always set # (Static analysis complains) self.visible = True # same self.color = (0, 0, 0) # maybe have an abstract method is_visible, get_color() self.process_obj_dict(obj, safety_radius_mult) self.domain_rand = domain_rand self.angle = self.y_rot * (math.pi / 180) self.generate_geometry() def generate_geometry(self): # Find corners and normal vectors assoc w. object self.obj_corners = generate_corners(self.pos, self.min_coords, self.max_coords, self.angle, self.scale) self.obj_norm = generate_norm(self.obj_corners) def process_obj_dict(self, obj, safety_radius_mult): self.kind = obj['kind'] self.mesh = obj['mesh'] self.pos = obj['pos'] self.scale = obj['scale'] self.y_rot = obj['y_rot'] self.optional = obj['optional'] self.min_coords = obj['mesh'].min_coords self.max_coords = obj['mesh'].max_coords self.static = obj['static'] self.safety_radius = safety_radius_mult *\ calculate_safety_radius(self.mesh, self.scale) def render(self, draw_bbox): """ Renders the object to screen """ if not self.visible: return from pyglet import gl # Draw the bounding box if draw_bbox: gl.glColor3f(1, 0, 0) gl.glBegin(gl.GL_LINE_LOOP) gl.glVertex3f(self.obj_corners.T[0, 0], 0.01, self.obj_corners.T[1, 0]) gl.glVertex3f(self.obj_corners.T[0, 1], 0.01, self.obj_corners.T[1, 1]) gl.glVertex3f(self.obj_corners.T[0, 2], 0.01, self.obj_corners.T[1, 2]) gl.glVertex3f(self.obj_corners.T[0, 3], 0.01, self.obj_corners.T[1, 3]) gl.glEnd() gl.glPushMatrix() gl.glTranslatef(*self.pos) gl.glScalef(self.scale, self.scale, self.scale) gl.glRotatef(self.y_rot, 0, 1, 0) gl.glColor3f(*self.color) self.mesh.render() gl.glPopMatrix() # Below are the functions that need to # be reimplemented for any dynamic object def check_collision(self, agent_corners, agent_norm): """ See if the agent collided with this object For static, return false (static collisions checked w numpy in a batch operation) """ if not self.static: raise NotImplementedError return False def proximity(self, agent_pos, agent_safety_rad): """ See if the agent is too close to this object For static, return 0 (static safedriving checked w numpy in a batch operation) """ if not self.static: raise NotImplementedError return 0.0 def step(self, delta_time): """ Use a motion model to move the object in the world """ if not self.static: raise NotImplementedError class DuckiebotObj(WorldObj): def __init__(self, obj, domain_rand, safety_radius_mult, wheel_dist, robot_width, robot_length, gain=2.0, trim=0.0, radius=0.0318, k=27.0, limit=1.0): WorldObj.__init__(self, obj, domain_rand, safety_radius_mult) if self.domain_rand: self.follow_dist = np.random.uniform(0.3, 0.4) self.velocity = np.random.uniform(0.05, 0.15) else: self.follow_dist = 0.3 self.velocity = 0.1 self.max_iterations = 1000 # TODO: Make these DR as well self.gain = gain self.trim = trim self.radius = radius self.k = k self.limit = limit self.wheel_dist = wheel_dist self.robot_width = robot_width self.robot_length = robot_length # FIXME: this does not follow the same signature as WorldOb def step(self, delta_time, closest_curve_point, objects): """ Take a step, implemented as a PID controller """ # Find the curve point closest to the agent, and the tangent at that point closest_point, closest_tangent = closest_curve_point(self.pos, self.angle) iterations = 0 lookup_distance = self.follow_dist curve_point = None while iterations < self.max_iterations: # Project a point ahead along the curve tangent, # then find the closest point to to that follow_point = closest_point + closest_tangent * lookup_distance curve_point, _ = closest_curve_point(follow_point, self.angle) # If we have a valid point on the curve, stop if curve_point is not None: break iterations += 1 lookup_distance *= 0.5 # Compute a normalized vector to the curve point point_vec = curve_point - self.pos point_vec /= np.linalg.norm(point_vec) dot = np.dot(self.get_right_vec(self.angle), point_vec) steering = self.gain * -dot self._update_pos([self.velocity, steering], delta_time) def get_dir_vec(self, angle): x = math.cos(angle) z = -math.sin(angle) return np.array([x, 0, z]) def get_right_vec(self, angle): x = math.sin(angle) z = math.cos(angle) return np.array([x, 0, z]) def check_collision(self, agent_corners, agent_norm): """ See if the agent collided with this object """ return intersects_single_obj( agent_corners, self.obj_corners.T, agent_norm, self.obj_norm ) def proximity(self, agent_pos, agent_safety_rad): """ See if the agent is too close to this object based on a heuristic for the "overlap" between their safety circles """ d = np.linalg.norm(agent_pos - self.pos) score = d - agent_safety_rad - self.safety_radius return min(0, score) def _update_pos(self, action, deltaTime): vel, angle = action # assuming same motor constants k for both motors k_r = self.k k_l = self.k # adjusting k by gain and trim k_r_inv = (self.gain + self.trim) / k_r k_l_inv = (self.gain - self.trim) / k_l omega_r = (vel + 0.5 * angle * self.wheel_dist) / self.radius omega_l = (vel - 0.5 * angle * self.wheel_dist) / self.radius # conversion from motor rotation rate to duty cycle u_r = omega_r * k_r_inv u_l = omega_l * k_l_inv # limiting output to limit, which is 1.0 for the duckiebot u_r_limited = max(min(u_r, self.limit), -self.limit) u_l_limited = max(min(u_l, self.limit), -self.limit) # If the wheel velocities are the same, then there is no rotation if u_l_limited == u_r_limited: self.pos = self.pos + deltaTime * u_l_limited * self.get_dir_vec(self.angle) return # Compute the angular rotation velocity about the ICC (center of curvature) w = (u_r_limited - u_l_limited) / self.wheel_dist # Compute the distance to the center of curvature r = (self.wheel_dist * (u_l_limited + u_r_limited)) / (2 * (u_l_limited - u_r_limited)) # Compute the rotation angle for this time step rotAngle = w * deltaTime # Rotate the robot's position around the center of rotation r_vec = self.get_right_vec(self.angle) px, py, pz = self.pos cx = px + r * r_vec[0] cz = pz + r * r_vec[2] npx, npz = rotate_point(px, pz, cx, cz, rotAngle) # Update position self.pos = np.array([npx, py, npz]) # Update the robot's direction angle self.angle += rotAngle self.y_rot += rotAngle * 180 / np.pi # Recompute the bounding boxes (BB) for the duckiebot self.obj_corners = agent_boundbox( self.pos, self.robot_width, self.robot_length, self.get_dir_vec(self.angle), self.get_right_vec(self.angle) ) class DuckieObj(WorldObj): def __init__(self, obj, domain_rand, safety_radius_mult, walk_distance): WorldObj.__init__(self, obj, domain_rand, safety_radius_mult) self.walk_distance = walk_distance + 0.25 # Dynamic duckie stuff # Randomize velocity and wait time if self.domain_rand: self.pedestrian_wait_time = np.random.randint(3, 20) self.vel = np.abs(np.random.normal(0.02, 0.005)) else: self.pedestrian_wait_time = 8 self.vel = 0.02 # Movement parameters self.heading = heading_vec(self.angle) self.start = np.copy(self.pos) self.center = self.pos self.pedestrian_active = False # Walk wiggle parameter self.wiggle = np.random.choice([14, 15, 16], 1) self.wiggle = np.pi / self.wiggle self.time = 0 def check_collision(self, agent_corners, agent_norm): """ See if the agent collided with this object """ return intersects_single_obj( agent_corners, self.obj_corners.T, agent_norm, self.obj_norm ) def proximity(self, agent_pos, agent_safety_rad): """ See if the agent is too close to this object based on a heuristic for the "overlap" between their safety circles """ d = np.linalg.norm(agent_pos - self.center) score = d - agent_safety_rad - self.safety_radius return min(0, score) def step(self, delta_time): """ Use a motion model to move the object in the world """ self.time += delta_time # If not walking, no need to do anything if not self.pedestrian_active: self.pedestrian_wait_time -= delta_time if self.pedestrian_wait_time <= 0: self.pedestrian_active = True return # Update centers and bounding box vel_adjust = self.heading * self.vel self.center += vel_adjust self.obj_corners += vel_adjust[[0, -1]] distance = np.linalg.norm(self.center - self.start) if distance > self.walk_distance: self.finish_walk() self.pos = self.center angle_delta = self.wiggle * math.sin(48 * self.time) self.y_rot = (self.angle + angle_delta) * (180 / np.pi) self.obj_norm = generate_norm(self.obj_corners) def finish_walk(self): """ After duckie crosses, update relevant attributes (vel, rot, wait time until next walk) """ self.start = np.copy(self.center) self.angle += np.pi self.pedestrian_active = False if self.domain_rand: # Assign a random velocity (in opp. direction) and a wait time # TODO: Fix this: This will go to 0 over time self.vel = -1 * np.sign(self.vel) * np.abs(np.random.normal(0.02, 0.005)) self.pedestrian_wait_time = np.random.randint(3, 20) else: # Just give it the negative of its current velocity self.vel *= -1 self.pedestrian_wait_time = 8 class TrafficLightObj(WorldObj): def __init__(self, obj, domain_rand, safety_radius_mult): WorldObj.__init__(self, obj, domain_rand, safety_radius_mult) self.texs = [ load_texture(get_file_path("textures", "trafficlight_card0", "jpg")), load_texture(get_file_path("textures", "trafficlight_card1", "jpg")) ] self.time = 0 # Frequency and current pattern of the lights if self.domain_rand: self.freq = np.random.randint(4, 7) self.pattern = np.random.randint(0, 2) else: self.freq = 5 self.pattern = 0 # Use the selected pattern self.mesh.textures[0] = self.texs[self.pattern] def step(self, delta_time): """ Changes the light color periodically """ self.time += delta_time if round(self.time, 3) % self.freq == 0: # Swap patterns self.pattern ^= 1 self.mesh.textures[0] = self.texs[self.pattern] def is_green(self, direction='N'): if direction == 'N' or direction == 'S': if self.y_rot == 45 or self.y_rot == 135: return self.pattern == 0 elif self.y_rot == 225 or self.y_rot == 315: return self.pattern == 1 elif direction == 'E' or direction == 'W': if self.y_rot == 45 or self.y_rot == 135: return self.pattern == 1 elif self.y_rot == 225 or self.y_rot == 315: return self.pattern == 0 return False,是定义了车子的运动学属性吗
最新发布
07-12
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值